Add IR, encoder, motor, display

compLib/Motor.py

@@ -1,49 +1,76 @@
 import atexit
+from enum import Enum
 
 from compLib.LogstashLogging import Logging
+from compLib.Spi import Spi, Register
 
 MOTOR_COUNT = 4
-MAX_MOTOR_SPEED = 4095.0
+MAX_MOTOR_SPEED = 65535
 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
 
 
+class MotorMode(Enum):
+    COAST = 0,
+    FORWARD = 1,
+    BACKWARD = 2,
+    BREAK = 3
+
+
 class Motor(object):
     """Class used to control the motors
     """
 
+    @staticmethod
+    def pwm(port: int, pwm: int, mode: MotorMode):
+        """Set specified motor to a specific pwm value and motor mode
+
+        :param port: Port, which the motor is connected to. 1-4 allowed
+        :param pwm: Raw PWM value which the motor should be set to. From 0 to 2^16 - 1
+        :param mode: Motor mode. See enum MotorMode for more info
+        :raises: IndexError
+        """
+        if port <= 0 or port > MOTOR_COUNT:
+            raise IndexError("Invalid Motor port specified!")
+
+        if port == 1:
+            Spi.write(Register.MOTOR_1_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_1_CTRL, 1, mode)
+        elif port == 2:
+            Spi.write(Register.MOTOR_2_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_2_CTRL, 1, mode)
+        elif port == 3:
+            Spi.write(Register.MOTOR_3_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_3_CTRL, 1, mode)
+        elif port == 4:
+            Spi.write(Register.MOTOR_4_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_4_CTRL, 1, mode)
+
     @staticmethod
     def power(port: int, percent: float):
         """Set specified motor to percentage power
 
-        :param port: Port, which the motor is connected to. 0-3, 0 -> top left, 3 -> top right
+        :param port: Port, which the motor is connected to. 1-4
         :param percent: Percentage of max speed. between -100 and 100
         :raises: IndexError
         """
         Logging.get_logger().debug(f"Motor.power {port} {percent}")
 
-        if port < 0 or port >= MOTOR_COUNT:
+        if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid Motor port specified!")
 
         if percent < -100 or percent > 100:
             raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!")
 
-        forward = True
+        mode = MotorMode.COAST
         if percent < 0:
             percent = abs(percent)
-            forward = False
+            mode = MotorMode.BACKWARD
+        elif percent > 0:
+            mode = MotorMode.FORWARD
 
-        # bottom left motor is inverted - REEEEEEEEEEEE
-        if port == 1:
-            forward = not forward
+        pwm = percent * MOTOR_PERCENTAGE_MULT
 
-        adjusted_speed = int(min(max(0.0, percent), 100.0) * MOTOR_PERCENTAGE_MULT)
-
-        if forward:
-            pwm.setMotorPwm(port * 2, adjusted_speed)
-            pwm.setMotorPwm(port * 2 + 1, 0)
-        else:
-            pwm.setMotorPwm(port * 2, 0)
-            pwm.setMotorPwm(port * 2 + 1, adjusted_speed)
+        Motor.pwm(port, int(pwm), mode)
 
     @staticmethod
     def all_off():
@@ -52,8 +79,17 @@ class Motor(object):
         """
         Logging.get_logger().debug(f"Motor.all_off")
 
-        for i in range(0, MOTOR_COUNT):
-            Motor.power(i, 0)
+        for i in range(1, MOTOR_COUNT + 1):
+            Motor.active_break(i)
+
+    @staticmethod
+    def active_break(port: int):
+        """
+        Actively break with a specific motor
+
+        :param port: Port, which the motor is connected to. 1-4
+        """
+        Motor.pwm(port, 0, MotorMode.BREAK)
 
 
 atexit.register(Motor.all_off)