Created
February 6, 2025 05:32
-
-
Save yuanoook/2fe1e55f9b5401ff9fb98351e938c103 to your computer and use it in GitHub Desktop.
Snake Robot Basics
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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