Testing servo driver

driver.py

@@ -0,0 +1,41 @@
+# https://learn.adafruit.com/16-channel-pwm-servo-driver/python-circuitpython
+
+import time
+from adafruit_servokit import ServoKit
+
+MIN = 550
+MAX = 2450
+CHANNELS = 16
+
+kit = ServoKit(channels=CHANNELS, frequency=49)
+
+for i in range(CHANNELS):
+    kit.servo[i].angle = 0
+
+time.sleep(2)
+
+# Testing the accuracy of the lib
+num = 14
+
+kit.servo[num].angle = 0
+time.sleep(2)
+
+kit.servo[num].angle = 90
+time.sleep(2)
+
+kit.servo[num].angle = 30
+time.sleep(2)
+
+kit.servo[num].angle = 120
+time.sleep(2)
+
+kit.servo[num].angle = 60
+time.sleep(2)
+
+kit.servo[num].angle = 150
+time.sleep(1.5)
+
+kit.servo[num].angle = 90
+time.sleep(1.5)
+
+kit.servo[num].angle = 180

gpt-servo.py

@@ -0,0 +1,96 @@
+import time
+from adafruit_servokit import ServoKit
+
+class MultiSmoothServoController:
+    def __init__(self, channels=16, min_pulse=500, max_pulse=2500):
+        self.kit = ServoKit(channels=channels)
+        self.channels = channels
+        self.min_pulse = min_pulse
+        self.max_pulse = max_pulse
+        
+        # Track last angle and pulse per servo
+        self._last_angles = [None] * channels
+        self._last_pulses = [None] * channels
+        
+        # Set pulse width range for all channels upfront
+        for ch in range(channels):
+            self.kit.servo[ch].set_pulse_width_range(min_pulse, max_pulse)
+    
+    def angle_to_pulse(self, angle):
+        """Convert angle (0-180) to pulse width in microseconds"""
+        return int(self.min_pulse + (self.max_pulse - self.min_pulse) * angle / 180)
+    
+    def set_angle(self, channel, angle):
+        """Set angle for a single servo channel, avoid jitter"""
+        if channel < 0 or channel >= self.channels:
+            raise ValueError(f"Channel must be between 0 and {self.channels - 1}")
+        if angle < 0 or angle > 180:
+            raise ValueError("Angle must be between 0 and 180")
+        
+        pulse = self.angle_to_pulse(angle)
+        if self._last_angles[channel] != angle or self._last_pulses[channel] != pulse:
+            self.kit.servo[channel].pulse_width = pulse
+            self._last_angles[channel] = angle
+            self._last_pulses[channel] = pulse
+    
+    def smooth_move(self, channel, start_angle, end_angle, step=1, delay=0.02):
+        """Smoothly move one servo from start_angle to end_angle"""
+        if start_angle < end_angle:
+            angles = range(start_angle, end_angle + 1, step)
+        else:
+            angles = range(start_angle, end_angle - 1, -step)
+        
+        for angle in angles:
+            self.set_angle(channel, angle)
+            time.sleep(delay)
+    
+    def smooth_move_all(self, start_angles, end_angles, step=1, delay=0.02):
+        """
+        Smoothly move all servos from their respective start_angles to end_angles.
+        Both start_angles and end_angles should be lists/tuples of length = number of channels.
+        """
+        if len(start_angles) != self.channels or len(end_angles) != self.channels:
+            raise ValueError("start_angles and end_angles must have length equal to number of channels")
+        
+        max_steps = 0
+        # Calculate how many steps each servo needs and track max
+        steps_per_servo = []
+        for start, end in zip(start_angles, end_angles):
+            steps = abs(end - start) // step
+            steps_per_servo.append(steps)
+            if steps > max_steps:
+                max_steps = steps
+        
+        for i in range(max_steps + 1):
+            for ch in range(self.channels):
+                start = start_angles[ch]
+                end = end_angles[ch]
+                
+                if start < end:
+                    angle = min(start + i * step, end)
+                else:
+                    angle = max(start - i * step, end)
+                
+                self.set_angle(ch, angle)
+            time.sleep(delay)
+
+    def stop_all(self):
+        """Stop pulses to all servos (optional)"""
+        for ch in range(self.channels):
+            self.kit.servo[ch].pulse_width = 0
+            self._last_angles[ch] = None
+            self._last_pulses[ch] = None
+
+controller = MultiSmoothServoController()
+
+# Move servo channel 2 smoothly from 0 to 180 degrees
+controller.smooth_move(channel=2, start_angle=0, end_angle=180)
+
+# Smoothly move multiple servos simultaneously:
+start_positions = [0] * 16          # all servos at 0°
+end_positions = [90, 45, 180, 0, 30, 60, 90, 120, 150, 180, 0, 90, 45, 135, 180, 0]
+
+controller.smooth_move_all(start_positions, end_positions)
+
+# Stop all servos
+controller.stop_all()

test.py

@@ -0,0 +1,54 @@
+import time
+from adafruit_servokit import ServoKit
+
+# This is set to zero for the max range
+OFFSET = -10
+
+MIN = 500 + OFFSET
+MAX = 2500 - OFFSET
+CHANNELS = 16
+RANGE = 180
+INIT = 0
+FREQUENCY = 50
+
+# Setup the kit
+kit = ServoKit(channels=CHANNELS, frequency=FREQUENCY)
+
+for i in range(CHANNELS):
+    kit.servo[i].set_pulse_width_range(MIN, MAX)
+    kit.servo[i].actuation_range = RANGE
+    kit.servo[i].angle = INIT
+
+time.sleep(2)
+
+# Testing the accuracy of the lib
+NUM = 8
+OFFSET = 5
+
+# Set the angle
+def sa(angle=0, sleep=1.5):
+    print(f"Set angle of pin {NUM+1} to {angle}.")
+    kit.servo[NUM].angle = angle + OFFSET
+    time.sleep(0.01)
+    kit.servo[NUM].angle = angle
+    kit.servo[NUM].angle = angle - OFFSET
+    time.sleep(0.01)
+    kit.servo[NUM].angle = angle
+    time.sleep(sleep)
+
+sa(80)
+exit()
+
+#for i in range(180):
+    #sa(i, 0)
+    #time.sleep(0.1)
+
+
+# Run the actual testing
+sa(10)
+sa(90)
+sa(30)
+sa(170)
+sa(60)
+sa(150)
+sa(90, 0)