From 7ec6854ac6d1121a345fb511a4bca9ba5eac8c47 Mon Sep 17 00:00:00 2001
From: koka <koka@test.at>
Date: Thu, 28 Jan 2021 22:12:33 +0000
Subject: [PATCH] Add servos

---
 compLib/PCA9685.py |  5 ++---
 compLib/Servo.py   | 26 ++++++++++++++++----------
 2 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/compLib/PCA9685.py b/compLib/PCA9685.py
index b2b3513..1389e55 100644
--- a/compLib/PCA9685.py
+++ b/compLib/PCA9685.py
@@ -47,12 +47,11 @@ class PCA9685:
         prescaleval /= 4096.0  # 12-bit
         prescaleval /= float(freq)
         prescaleval -= 1.0
-        prescale = math.floor(prescaleval + 0.5)
 
         oldmode = self.read(self.__MODE1)
         newmode = (oldmode & 0x7F) | 0x10  # sleep
         self.write(self.__MODE1, newmode)  # go to sleep
-        self.write(self.__PRESCALE, int(math.floor(prescale)))
+        self.write(self.__PRESCALE, int(round(prescaleval)))
         self.write(self.__MODE1, oldmode)
         time.sleep(0.005)
         self.write(self.__MODE1, oldmode | 0x80)
@@ -69,7 +68,7 @@ class PCA9685:
 
     def setServoPulse(self, channel, pulse):
         "Sets the Servo Pulse,The PWM frequency must be 50HZ"
-        pulse = pulse * 4096 / 20000  # PWM frequency is 50HZ,the period is 20000us
+        pulse = float(pulse) * (4096.0 / 20000.0)  # PWM frequency is 50HZ,the period is 20000us
         self.setPWM(channel, 0, int(pulse))
 
 
diff --git a/compLib/Servo.py b/compLib/Servo.py
index d4a0d58..b341d15 100644
--- a/compLib/Servo.py
+++ b/compLib/Servo.py
@@ -1,30 +1,36 @@
-from compLib.PCA9685 import PCA9685
+from PCA9685 import PCA9685
+
+SERVO_COUNT = 10
 
 pwm = PCA9685(0x40, debug=True)
 pwm.setPWMFreq(50)
 
+MIN_ANGLE = -90.0
+MAX_ANGLE = 90.0
+
 
 class Servo:
     """Control the servo ports on the robot
     """
 
     @staticmethod
-    def set_position(channel: int, angle: int):
+    def set_position(channel: int, angle: int, offset: float =90):
         """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))
+        if channel < 0 or channel >= SERVO_COUNT:
+            raise IndexError("Invalid Servo channel specified!")
+        
+        angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
+        
+        pwm.setServoPulse(channel + 8, 500+int((angle+offset)/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)
+        
+        Servo.set_position(0, 0)
+        Servo.set_position(1, 0)