From 1058fd505fa5be1e86e17277c4b974d2708fa3e4 Mon Sep 17 00:00:00 2001 From: Dlr Rpi Date: Thu, 23 Oct 2025 15:30:06 +0200 Subject: [PATCH] Testing servo driver --- driver.py | 41 ++++++++++++++++++++++ gpt-servo.py | 96 ++++++++++++++++++++++++++++++++++++++++++++++++++++ test.py | 54 +++++++++++++++++++++++++++++ 3 files changed, 191 insertions(+) create mode 100644 driver.py create mode 100644 gpt-servo.py create mode 100644 test.py diff --git a/driver.py b/driver.py new file mode 100644 index 0000000..7b594b9 --- /dev/null +++ b/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 diff --git a/gpt-servo.py b/gpt-servo.py new file mode 100644 index 0000000..8bd5b7c --- /dev/null +++ b/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() diff --git a/test.py b/test.py new file mode 100644 index 0000000..ba5c9f7 --- /dev/null +++ b/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)