Mecanum platform, XBox controller controlled

[MichaelVerschaeve]

(click image to see on Youtube)

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.iodevices import XboxController
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.tools import wait, StopWatch

#Setup 
hub = TechnicHub()
xbox = XboxController()

front_left  = Motor(Port.C, Direction.COUNTERCLOCKWISE)
front_right = Motor(Port.A, Direction.CLOCKWISE)
rear_left   = Motor(Port.D, Direction.COUNTERCLOCKWISE)
rear_right  = Motor(Port.B, Direction.CLOCKWISE)

def drive_mecanum(forward, strafe, turn):
    """
    Calculates and applies voltage/speed settings to 4 independent wheels.
    Inputs should range from -100 to 100 for percentage-based scaling.
    """
    # Standard kinematics equations for Mecanum setups
    fl_speed = forward + strafe + turn
    fr_speed = forward - strafe - turn
    rl_speed = forward - strafe + turn
    rr_speed = forward + strafe - turn
    
    # Find the maximum theoretical output to avoid exceeding maximum motor limits
    max_val = max(abs(fl_speed), abs(fr_speed), abs(rl_speed), abs(rr_speed))
    
    # Scale down proportionately if any single equation exceeds a power of 100%
    if max_val > 100:
        fl_speed = (fl_speed / max_val) * 100
        fr_speed = (fr_speed / max_val) * 100
        rl_speed = (rl_speed / max_val) * 100
        rr_speed = (rr_speed / max_val) * 100
        
    # Send calculated percentage directly to the motor DC voltage control
    front_left.dc(fl_speed)
    front_right.dc(fr_speed)
    rear_left.dc(rl_speed)
    rear_right.dc(rr_speed)

# The main program starts here.
while True:
    speedDrive = xbox.joystick_right()[1]
    speedStrafe = xbox.joystick_right()[0]
    speedRotate = xbox.triggers()[1] - xbox.triggers()[0]
    


    # Execute the layout matrix
    drive_mecanum(speedDrive, speedStrafe, speedRotate)

    wait(50)

[dlech]

Nice. Went for the "pure" LEGO omni wheels, I see. 😄

[MichaelVerschaeve]

Yes, own design but inspired by those designed by Yoshihito ISOGAWA