Testing servo driver

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()