Snake Robot Basics

boot.py

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)

engine.py

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)