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