from machine import Pin, PWM
import time
import math
class Servo:
def __init__(self, pin: int, inverted: bool = False,
min_angle: float = 0, max_angle: float = 180):
self.min_angle = min_angle
self.max_angle = max_angle
self.min_duty = 600000
self.max_duty = 2500000
self.resting = (min_angle + max_angle) / 2 # Set resting position to midpoint
self.current_duty = self.angle_to_duty(self.resting)
self.current_angle = self.resting
self.inverted = inverted
self.pin = Pin(pin)
self.pwm = PWM(self.pin)
self.pwm.freq(50)
def angle_to_duty(self, angle: float) -> int:
"""Convert an angle to a duty cycle."""
duty = (angle - self.min_angle) / (self.max_angle - self.min_angle) * (self.max_duty - self.min_duty) + self.min_duty
return int(duty)
def move(self, angle: float):
"""Move the servo to a specific angle."""
angle = max(self.min_angle, min(angle, self.max_angle))
if self.inverted:
angle = self.max_angle - angle + self.min_angle
self.current_duty = self.angle_to_duty(angle)
self.pwm.duty_ns(self.current_duty)
self.current_angle = angle
def rest(self):
"""Move the servo to its resting position."""
self.move(self.resting)
def minimum(self):
"""Move the servo to its minimum angle."""
self.move(self.min_angle)
def maximum(self):
"""Move the servo to its maximum angle."""
self.move(self.max_angle)
def _lerp(self, t: float, start: float, end: float) -> float:
"""Linear interpolation."""
return start + (end - start) * t
# Additional easing functions
def _ease_in_quadratic(self, t: float) -> float:
return t * t
def _ease_out_quadratic(self, t: float) -> float:
return -t * (t - 2)
def _ease_in_out_quadratic(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * t * t
t -= 1
return -0.5 * (t * (t - 2) - 1)
def _ease_in_cubic(self, t: float) -> float:
return t * t * t
def _ease_out_cubic(self, t: float) -> float:
t -= 1
return t * t * t + 1
def _ease_in_out_cubic(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * t * t * t
t -= 2
return 0.5 * (t * t * t + 2)
def _ease_in_quartic(self, t: float) -> float:
return t * t * t * t
def _ease_out_quartic(self, t: float) -> float:
t -= 1
return 1 - t * t * t * t
def _ease_in_out_quartic(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * t * t * t * t
t -= 2
return -0.5 * (t * t * t * t - 2)
def _ease_in_quintic(self, t: float) -> float:
return t * t * t * t * t
def _ease_out_quintic(self, t: float) -> float:
t -= 1
return t * t * t * t * t + 1
def _ease_in_out_quintic(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * t * t * t * t * t
t -= 2
return 0.5 * (t * t * t * t * t + 2)
def _ease_in_exponential(self, t: float) -> float:
return 2 ** (10 * (t - 1))
def _ease_out_exponential(self, t: float) -> float:
return 1 - 2 ** (-10 * t)
def _ease_in_out_exponential(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * 2 ** (10 * (t - 1))
t -= 1
return 0.5 * (2 - 2 ** (-10 * t))
def _ease_in_circular(self, t: float) -> float:
return 1 - math.sqrt(1 - t * t)
def _ease_out_circular(self, t: float) -> float:
t -= 1
return math.sqrt(1 - t * t)
def _ease_in_out_circular(self, t: float) -> float:
t /= 0.5
if t < 1:
return 0.5 * (1 - math.sqrt(1 - t * t))
t -= 2
return 0.5 * (math.sqrt(1 - t * t) + 1)
def _ease_in_sinusoidal(self, t: float) -> float:
return 1 - math.cos(t * math.pi / 2)
def _ease_out_sinusoidal(self, t: float) -> float:
return math.sin(t * math.pi / 2)
def _ease_in_out_sinusoidal(self, t: float) -> float:
return 0.5 * (1 - math.cos(math.pi * t))
def _ease_in_out_exponential(self, t: float) -> float:
if t < 0.5:
return 2 ** (10 * (2 * t - 1))
return 2 - 2 ** (-10 * (2 * t - 1))
def _ease_in_out_circular(self, t: float) -> float:
if t < 0.5:
return 0.5 * (1 - math.sqrt(1 - (2 * t) ** 2))
return 0.5 * (math.sqrt(1 - (-2 * t + 2) ** 2) + 1)
def _ease_out_elastic(self, t: float) -> float:
A = 1
B = 1
C = 0.5
if t < 1:
return 1 - A * 2 ** (-10 * t) * math.sin((t - B) * (2 * math.pi) / C)
return A * 2 ** (-10 * (t - 1)) * math.sin((t - B) * (2 * math.pi) / C)
def _ease_in_back(self, t: float) -> float:
s = 1.70158
return t * t * ((s + 1) * t - s)
def _ease_out_back(self, t: float) -> float:
s = 1.70158
t -= 1
return (t * t * ((s + 1) * t + s) + 1)
def _ease_in_out_back(self, t: float) -> float:
s = 1.70158 * 1.525
t /= 0.5
if t < 1:
return 0.5 * (t * t * ((s + 1) * t - s))
t -= 2
return 0.5 * (t * t * ((s + 1) * t + s) + 2)
def _ease_out_bounce(self, t: float) -> float:
if t < 0.2:
return 7.5625 * t * t
elif t < 0.4:
t -= 0.2
return 7.5625 * t * t + 0.75
elif t < 0.6:
t -= 0.4
return 7.5625 * t * t + 0.9375
t -= 0.6
return 7.5625 * t * t + 0.984375
def smooth_move(self, target_angle: float, duration: float, easing: str = 'linear'):
"""Smoothly move the servo to a target angle over a given duration with the specified easing."""
steps = 100
step_duration = duration / steps
start_angle = self.current_angle
delta_angle = target_angle - start_angle
for i in range(steps + 1):
t = i / steps
if easing == 'linear':
angle = self._lerp(t, start_angle, target_angle)
elif easing == 'ease_in_quadratic':
angle = self._lerp(self._ease_in_quadratic(t), start_angle, target_angle)
elif easing == 'ease_out_quadratic':
angle = self._lerp(self._ease_out_quadratic(t), start_angle, target_angle)
elif easing == 'ease_in_out_quadratic':
angle = self._lerp(self._ease_in_out_quadratic(t), start_angle, target_angle)
elif easing == 'ease_in_cubic':
angle = self._lerp(self._ease_in_cubic(t), start_angle, target_angle)
elif easing == 'ease_out_cubic':
angle = self._lerp(self._ease_out_cubic(t), start_angle, target_angle)
elif easing == 'ease_in_out_cubic':
angle = self._lerp(self._ease_in_out_cubic(t), start_angle, target_angle)
elif easing == 'ease_in_quartic':
angle = self._lerp(self._ease_in_quartic(t), start_angle, target_angle)
elif easing == 'ease_out_quartic':
angle = self._lerp(self._ease_out_quartic(t), start_angle, target_angle)
elif easing == 'ease_in_out_quartic':
angle = self._lerp(self._ease_in_out_quartic(t), start_angle, target_angle)
elif easing == 'ease_in_quintic':
angle = self._lerp(self._ease_in_quintic(t), start_angle, target_angle)
elif easing == 'ease_out_quintic':
angle = self._lerp(self._ease_out_quintic(t), start_angle, target_angle)
elif easing == 'ease_in_out_quintic':
angle = self._lerp(self._ease_in_out_quintic(t), start_angle, target_angle)
elif easing == 'ease_in_exponential':
angle = self._lerp(self._ease_in_exponential(t), start_angle, target_angle)
elif easing == 'ease_out_exponential':
angle = self._lerp(self._ease_out_exponential(t), start_angle, target_angle)
elif easing == 'ease_in_out_exponential':
angle = self._lerp(self._ease_in_out_exponential(t), start_angle, target_angle)
elif easing == 'ease_in_circular':
angle = self._lerp(self._ease_in_circular(t), start_angle, target_angle)
elif easing == 'ease_out_circular':
angle = self._lerp(self._ease_out_circular(t), start_angle, target_angle)
elif easing == 'ease_in_out_circular':
angle = self._lerp(self._ease_in_out_circular(t), start_angle, target_angle)
elif easing == 'ease_in_sinusoidal':
angle = self._lerp(self._ease_in_sinusoidal(t), start_angle, target_angle)
elif easing == 'ease_out_sinusoidal':
angle = self._lerp(self._ease_out_sinusoidal(t), start_angle, target_angle)
elif easing == 'ease_in_out_sinusoidal':
angle = self._lerp(self._ease_in_out_sinusoidal(t), start_angle, target_angle)
elif easing == 'ease_in_out_exponential':
angle = self._lerp(self._ease_in_out_exponential(t), start_angle, target_angle)
elif easing == 'ease_in_out_circular':
angle = self._lerp(self._ease_in_out_circular(t), start_angle, target_angle)
elif easing == 'ease_out_elastic':
angle = self._lerp(self._ease_out_elastic(t), start_angle, target_angle)
elif easing == 'ease_in_back':
angle = self._lerp(self._ease_in_back(t), start_angle, target_angle)
elif easing == 'ease_out_back':
angle = self._lerp(self._ease_out_back(t), start_angle, target_angle)
elif easing == 'ease_in_out_back':
angle = self._lerp(self._ease_in_out_back(t), start_angle, target_angle)
elif easing == 'ease_in_bounce':
angle = self._lerp(self._ease_in_bounce(t), start_angle, target_angle)
elif easing == 'ease_out_bounce':
angle = self._lerp(self._ease_out_bounce(t), start_angle, target_angle)
elif easing == 'ease_in_out_bounce':
angle = self._lerp(self._ease_in_out_bounce(t), start_angle, target_angle)
else:
raise ValueError("Unsupported easing type")
self.move(angle)
time.sleep(step_duration)
self.move(target_angle)
# Example usage for each new easing function:
claw = Servo(pin=0)
# Sinusoidal easing functions
claw.rest()
time.sleep(1)
claw.smooth_move(10, 1, easing='ease_in_sinusoidal') # Move to 10 degrees with ease_in_sinusoidal
time.sleep(1)
claw.smooth_move(120, 1, easing='ease_out_sinusoidal') # Move to 120 degrees with ease_out_sinusoidal
time.sleep(1)
claw.smooth_move(60, 1, easing='ease_in_out_sinusoidal') # Move to 60 degrees with ease_in_out_sinusoidal
time.sleep(1)