Rename folder

compLib/Motor.py

@@ -0,0 +1,51 @@
+import atexit
+
+from compLib.PCA9685 import PCA9685
+
+pwm = PCA9685(0x40, debug=True)
+pwm.setPWMFreq(50)
+
+MOTOR_COUNT = 4
+MAX_MOTOR_SPEED = 4095.0
+MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
+
+
+class Motor(object):
+    """Class used to control the motors
+    """
+
+    @staticmethod
+    def power(port: int, percent: int):
+        """Set specified motor to percentage power
+
+        :param port: Port, which the motor is connected to. 0-3, 0 -> top left, 3 -> top right
+        :param percent: Percentage of max speed. between -100 and 100
+        """
+        forward = True
+        if percent < 0:
+            percent = abs(percent)
+            forward = False
+
+        # bottom left motor is inverted - REEEEEEEEEEEE
+        if port == 1:
+            forward = not forward
+
+        adjusted_speed = int(min(max(0, percent), 100) * MOTOR_PERCENTAGE_MULT)
+
+        if forward:
+            pwm.setMotorPwm(port * 2, 0)
+            pwm.setMotorPwm(port * 2 + 1, adjusted_speed)
+        else:
+            pwm.setMotorPwm(port * 2, adjusted_speed)
+            pwm.setMotorPwm(port * 2 + 1, 0)
+
+    @staticmethod
+    def all_off():
+        """
+        Turns of all motors
+        """
+        for i in range(0, MOTOR_COUNT):
+            Motor.power(i, 0)
+
+
+atexit.register(Motor.all_off)