Rename folder

compLib/Servo.py

@@ -0,0 +1,30 @@
+from compLib.PCA9685 import PCA9685
+
+pwm = PCA9685(0x40, debug=True)
+pwm.setPWMFreq(50)
+
+
+class Servo:
+    """Control the servo ports on the robot
+    """
+
+    @staticmethod
+    def set_position(channel: int, angle: int):
+        """Set position of servo connected to port
+
+        :param channel: channel between 0 and 7
+        :param angle: Angle of servo
+        """
+        angle = abs(angle)
+
+        if channel == 0:
+            pwm.setServoPulse(8 + channel, 2500 - int(angle / 0.09))
+        elif channel < 8:
+            pwm.setServoPulse(8 + channel, 500 - int(angle / 0.09))
+
+    @staticmethod
+    def setup_position():
+        """Set position of servos to the position used during the setup process
+        """
+        pwm.setServoPulse(8, 1500)
+        pwm.setServoPulse(9, 1500)