75 lines
2.5 KiB
Python
75 lines
2.5 KiB
Python
|
# servo.py — MG995 (and other analog hobby servos), 50 Hz PWM
|
|||
|
|
from machine import Pin, PWM
|
|||
|
|
|
|||
|
|
# MG995: treat as standard 1.0–2.0 ms for ~0–180°; many units reach full travel closer to 0.5–2.5 ms.
|
|||
|
|
_DEFAULT_MIN_US = 1000
|
|||
|
|
_DEFAULT_MAX_US = 2000
|
|||
|
|
_WIDE_MIN_US = 500
|
|||
|
|
_WIDE_MAX_US = 2500
|
|||
|
|
|
|||
|
|
|
|||
|
|
class MG995:
|
|||
|
|
def __init__(
|
|||
|
|
self,
|
|||
|
|
pin,
|
|||
|
|
freq=50,
|
|||
|
|
min_us=_DEFAULT_MIN_US,
|
|||
|
|
max_us=_DEFAULT_MAX_US,
|
|||
|
|
angle_max=180,
|
|||
|
|
wide_range=False,
|
|||
|
|
):
|
|||
|
|
"""
|
|||
|
|
Drive one MG995 on `pin` (GPIO number or Pin).
|
|||
|
|
|
|||
|
|
`wide_range=True` uses ~500–2500 µs pulse limits (often needed for full
|
|||
|
|
mechanical sweep); default 1000–2000 µs is gentler on the gearbox.
|
|||
|
|
|
|||
|
|
`angle_max` is the upper limit passed to write_angle (default 180).
|
|||
|
|
"""
|
|||
|
|
if wide_range:
|
|||
|
|
min_us, max_us = _WIDE_MIN_US, _WIDE_MAX_US
|
|||
|
|
self._pin_id = pin
|
|||
|
|
self._freq = int(freq)
|
|||
|
|
self._min_us = int(min_us)
|
|||
|
|
self._max_us = int(max_us)
|
|||
|
|
self._angle_max = float(angle_max)
|
|||
|
|
self._pwm = PWM(Pin(pin), freq=self._freq, duty_u16=0)
|
|||
|
|
self._use_ns = hasattr(self._pwm, 'duty_ns')
|
|||
|
|
self._period_us = 1_000_000 // self._freq if self._freq > 0 else 20_000
|
|||
|
|
|
|||
|
|
def write_microseconds(self, us):
|
|||
|
|
"""Set pulse width in microseconds (clamped to configured min/max)."""
|
|||
|
|
us = max(self._min_us, min(self._max_us, int(us)))
|
|||
|
|
if self._use_ns:
|
|||
|
|
self._pwm.duty_ns(us * 1000)
|
|||
|
|
else:
|
|||
|
|
# Fraction of period → duty_u16 (ESP32-style full-scale mapping)
|
|||
|
|
self._pwm.duty_u16(max(0, min(65535, int(us * 65535 / self._period_us)))))
|
|||
|
|
|
|||
|
|
def write_angle(self, degrees):
|
|||
|
|
"""Map `degrees` (0 … angle_max) to pulse between min_us and max_us."""
|
|||
|
|
a = max(0.0, min(self._angle_max, float(degrees)))
|
|||
|
|
span = self._max_us - self._min_us
|
|||
|
|
us = self._min_us + int(round(span * (a / self._angle_max)))
|
|||
|
|
self.write_microseconds(us)
|
|||
|
|
|
|||
|
|
def center(self):
|
|||
|
|
"""Mid pulse (~center position for default symmetric limits)."""
|
|||
|
|
self.write_microseconds((self._min_us + self._max_us) // 2)
|
|||
|
|
|
|||
|
|
def off(self):
|
|||
|
|
"""Stop PWM output (no holding torque from the signal)."""
|
|||
|
|
try:
|
|||
|
|
self._pwm.duty_u16(0)
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
try:
|
|||
|
|
if self._use_ns:
|
|||
|
|
self._pwm.duty_ns(0)
|
|||
|
|
except Exception:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
def deinit(self):
|
|||
|
|
self.off()
|
|||
|
|
self._pwm.deinit()
|