Skip to content

Instantly share code, notes, and snippets.

@yuanoook
Created February 6, 2025 05:32
Show Gist options
  • Save yuanoook/2fe1e55f9b5401ff9fb98351e938c103 to your computer and use it in GitHub Desktop.
Save yuanoook/2fe1e55f9b5401ff9fb98351e938c103 to your computer and use it in GitHub Desktop.
Snake Robot Basics
import machine
import time
import math
from engine import Engine
from joystick import Joystick
# Initialize the joystick
joystick = Joystick(x_pin=3, y_pin=2, switch_pin=1)
# Angle limit for the servos
angle_limit = 30
# Initialize the three motors
e8 = Engine(8, angle_limit)
e9 = Engine(9, angle_limit)
e10 = Engine(10, angle_limit)
# Assume joystick values range from 0 to 4095
JOYSTICK_MAX = 4095
def joystick_callback(x_value, y_value, switch_value):
if switch_value == 0:
# When the switch is pushed down, set all motor powers to -1
motor_powers = [-1, -1, -1]
# Set default values for angle and power
angle = 0
power = 0
else:
# Map x and y values to the range [-1, 1]
x_normalized = (x_value / JOYSTICK_MAX - 0.5) * 2
y_normalized = (y_value / JOYSTICK_MAX - 0.5) * 2
# Calculate the angle and power
angle = math.atan2(y_normalized, x_normalized)
# Reverse the direction of the angle
angle = -angle
power = math.sqrt(x_normalized**2 + y_normalized**2)
# Cap the power to 1
power = min(1, power)
# Calculate the power for each motor based on the angle
motor_angles = [0, 2 * math.pi / 3, 4 * math.pi / 3]
motor_powers = []
for motor_angle in motor_angles:
# Calculate the difference in angles
angle_diff = angle - motor_angle
# Normalize the angle difference to the range [-pi, pi]
angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi
# Calculate the power for this motor
motor_power = power * math.cos(angle_diff)
# Ensure motor power is in the range [-1, 1]
motor_power = max(-1, min(1, motor_power))
motor_powers.append(motor_power)
# Move the motors
e8.move(motor_powers[0])
e9.move(motor_powers[1])
e10.move(motor_powers[2])
print(f"Joystick: {x_value} {y_value} {switch_value}, Angle: {math.degrees(angle):.2f}°, Power: {power:.2f}")
print(f"Motor powers: {motor_powers[0]:.2f}, {motor_powers[1]:.2f}, {motor_powers[2]:.2f}")
def loop(func, *args, **kwargs):
try:
while True:
func(*args, **kwargs)
time.sleep(0.05)
except KeyboardInterrupt:
print("Interrupted!")
loop(joystick.read, joystick_callback)
import machine
import time
class Engine:
def __init__(self, pin, angle_range=90):
"""
Initialize the Engine with the specified pin and angle range.
Default angle range is 90 degrees (45 degrees on each side).
"""
self.servo = machine.PWM(machine.Pin(pin))
self.servo.freq(50)
self.angle_range = angle_range
self.center_angle = 90 # Assuming the center position is 90 degrees (neutral position)
def interval_mapping(self, x, in_min, in_max, out_min, out_max):
"""
Maps a value from one range to another.
"""
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def servo_write(self, angle):
"""
Moves the servo to a specific angle.
"""
pulse_width = self.interval_mapping(angle, 0, 180, 0.5, 2.5) # Map angle to pulse width in ms
duty = int(self.interval_mapping(pulse_width, 0, 20, 0, 65535)) # Map pulse width to duty cycle
self.servo.duty_u16(duty) # Set PWM duty cycle
def move_servo(self, x_value):
"""
Move the servo based on joystick X value.
x_value is assumed to be between 0 and 4095.
"""
# Calculate the total possible range of motion based on the configured angle range
half_range = self.angle_range / 2
angle = self.center_angle + int(((x_value / 4095) * self.angle_range) - half_range)
self.servo_write(angle)
def move(self, value):
"""
Move the servo based on a value in the range -1 to 1.
Maps the value to the range 0 to 4095 and calls move_servo.
"""
# Map the value from -1 to 1 to 0 to 4095
mapped_value = self.interval_mapping(value, -1, 1, 0, 4095)
self.move_servo(mapped_value)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment