110 lines
3.6 KiB
Python
110 lines
3.6 KiB
Python
from machine import Pin, PWM
|
||
import time, random
|
||
import urandom
|
||
|
||
def clamp(x, a, b):
|
||
return a if x < a else (b if x > b else x)
|
||
|
||
class Servo:
|
||
def __init__(self, pin_num, max_speed=120.0, max_accel=360.0, min_angle=0.0, max_angle=180.0, enabled=True):
|
||
self.pwm = PWM(Pin(pin_num))
|
||
self.pwm.freq(50)
|
||
|
||
# motion state
|
||
self.pos = 90.0
|
||
self.vel = 0.0
|
||
self.target = 90.0
|
||
|
||
# tuning
|
||
self.max_speed = float(max_speed)
|
||
self.max_accel = float(max_accel)
|
||
|
||
# limits
|
||
self.min_angle = float(min_angle)
|
||
self.max_angle = float(max_angle)
|
||
|
||
# tolerances
|
||
self.pos_tolerance = 0.6
|
||
self.vel_tolerance = 1.0
|
||
|
||
# debug flag
|
||
self.enabled = enabled
|
||
|
||
self._write_pwm(self.pos)
|
||
|
||
def set_target(self, angle):
|
||
# clamp target to servo’s declared limits
|
||
self.target = clamp(float(angle), self.min_angle, self.max_angle)
|
||
|
||
def update(self, dt):
|
||
if not self.enabled:
|
||
return
|
||
if dt <= 0:
|
||
return
|
||
|
||
error = self.target - self.pos
|
||
dist = abs(error)
|
||
# direction we need to move to reduce error
|
||
desired_dir = 0 if dist < 1e-9 else (1.0 if error > 0 else -1.0)
|
||
# current velocity sign
|
||
if abs(self.vel) < 1e-9:
|
||
vel_dir = 0.0
|
||
else:
|
||
vel_dir = 1.0 if self.vel > 0 else -1.0
|
||
|
||
# small-target case: if we're close & nearly stopped, snap cleanly
|
||
if dist <= self.pos_tolerance and abs(self.vel) <= self.vel_tolerance:
|
||
self.pos = self.target
|
||
self.vel = 0.0
|
||
self._write_pwm(self.pos)
|
||
return
|
||
|
||
# braking-first rule: if we're moving opposite desired direction, brake
|
||
if vel_dir != 0 and desired_dir != 0 and vel_dir != desired_dir:
|
||
# apply maximum braking (opposite current velocity)
|
||
accel = -vel_dir * self.max_accel
|
||
else:
|
||
# else decide accelerate or start decelerating so we stop exactly at target
|
||
stopping_dist = (self.vel * self.vel) / (2.0 * self.max_accel) # always >= 0
|
||
if dist > stopping_dist:
|
||
# still room to accelerate toward the target
|
||
accel = desired_dir * self.max_accel
|
||
else:
|
||
# we must decelerate to stop at the target
|
||
accel = -desired_dir * self.max_accel
|
||
|
||
# integrate velocity
|
||
new_vel = self.vel + accel * dt
|
||
|
||
# avoid sign flip jitter: if braking would cross zero in this step, clamp to zero
|
||
if self.vel > 0 and new_vel < 0:
|
||
new_vel = 0.0
|
||
elif self.vel < 0 and new_vel > 0:
|
||
new_vel = 0.0
|
||
|
||
# clamp speed to limits
|
||
new_vel = clamp(new_vel, -self.max_speed, self.max_speed)
|
||
|
||
# integrate position
|
||
self.vel = new_vel
|
||
self.pos = self.pos + self.vel * dt
|
||
|
||
# clamp position to valid servo range (avoid writing out-of-bounds)
|
||
self.pos = clamp(self.pos, 0.0, 180.0)
|
||
|
||
# final small-check: if we are extremely close, snap to target to avoid tiny oscillation
|
||
# if abs(self.target - self.pos) <= self.pos_tolerance and abs(self.vel) <= self.vel_tolerance:
|
||
# self.pos = self.target
|
||
# self.vel = 0.0
|
||
|
||
self._write_pwm(self.pos)
|
||
|
||
def _write_pwm(self, angle):
|
||
# map 0..180 -> 500..2500 us pulse width (20 ms period)
|
||
min_us = 500.0
|
||
max_us = 2500.0
|
||
us = min_us + (max_us - min_us) * (angle / 180.0)
|
||
# convert to 16-bit duty for 20 ms period
|
||
duty = int(us * 65535.0 / 20000.0)
|
||
self.pwm.duty_u16(duty)
|