|
| 1 | +from pybricks.pupdevices import Motor, Remote |
| 2 | +from pybricks.parameters import Button, Color, Direction, Port |
| 3 | +from pybricks.tools import wait, StopWatch |
| 4 | + |
| 5 | +# Connect to the remote. |
| 6 | +remote = Remote() |
| 7 | + |
| 8 | +# Default speed and acceleration. You can change |
| 9 | +# them to make it more realistic. |
| 10 | +SWITCH_SPEED = 720 |
| 11 | +DRIVE_SPEED = 1000 |
| 12 | +DRIVE_ACCELERATION = 2500 |
| 13 | +FUNCTION_POWER = 100 |
| 14 | + |
| 15 | +# Initialize the motors. |
| 16 | +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) |
| 17 | +right_motor = Motor(Port.B) |
| 18 | +function_motor = Motor(Port.C) |
| 19 | +switch_motor = Motor(Port.D) |
| 20 | + |
| 21 | +left_motor.control.limits(acceleration=DRIVE_ACCELERATION) |
| 22 | +right_motor.control.limits(acceleration=DRIVE_ACCELERATION) |
| 23 | + |
| 24 | +# Buttons paired with the green button to make a command. |
| 25 | +COMMANDS = { |
| 26 | + Button.LEFT_PLUS: ("Blade up/down", 0, Color.BLUE), |
| 27 | + Button.LEFT_MINUS: ("Blade tilt", 270, Color.RED), |
| 28 | + Button.RIGHT_PLUS: ("Ladder", 180, Color.YELLOW), |
| 29 | + Button.RIGHT_MINUS: ("Ripper", 90, Color.GREEN), |
| 30 | +} |
| 31 | + |
| 32 | +# Find the right end left endstop positions |
| 33 | +right_end = switch_motor.run_until_stalled(500, duty_limit=50) |
| 34 | +left_end = switch_motor.run_until_stalled(-500, duty_limit=50) |
| 35 | + |
| 36 | +# The full switch motion ranges 270 degrees. Everything beyond that |
| 37 | +# is play towards the endstops, equal in both directions. Since we |
| 38 | +# are currently at the left endpoint, we can reset the angle |
| 39 | +# accordingly. This way, the functions # are simply at the |
| 40 | +# relative positions 0, 90, 180, 270. |
| 41 | +switch_motor.reset_angle(-(right_end - left_end - 270) / 2) |
| 42 | + |
| 43 | + |
| 44 | +def switch_target(target_angle): |
| 45 | + # Try up to 5 times. |
| 46 | + for i in range(5): |
| 47 | + |
| 48 | + # Keep track of how long we've tried. |
| 49 | + watch = StopWatch() |
| 50 | + switch_motor.run_target(SWITCH_SPEED, target, wait=False) |
| 51 | + |
| 52 | + # Wait until the stopwatch times out. |
| 53 | + while watch.time() < 2000: |
| 54 | + wait(10) |
| 55 | + # We're done if the motor is on target, so exit |
| 56 | + # this function. |
| 57 | + if switch_motor.control.done(): |
| 58 | + return |
| 59 | + |
| 60 | + # Otherwise, we got stuck, so try wiggling around to |
| 61 | + # release it. |
| 62 | + print("Getting unstuck.") |
| 63 | + switch_motor.run_target(SWITCH_SPEED, 0, wait=False) |
| 64 | + wait(1500) |
| 65 | + switch_motor.run_target(SWITCH_SPEED, 270, wait=False) |
| 66 | + wait(1500) |
| 67 | + |
| 68 | + |
| 69 | +while True: |
| 70 | + |
| 71 | + # Check which buttons are pressed |
| 72 | + wait(10) |
| 73 | + pressed = remote.buttons.pressed() |
| 74 | + |
| 75 | + # If the center button is pressed, process the |
| 76 | + # corresponding command. |
| 77 | + if Button.CENTER in pressed: |
| 78 | + |
| 79 | + # Stop driving |
| 80 | + right_motor.stop() |
| 81 | + left_motor.stop() |
| 82 | + function_motor.stop() |
| 83 | + |
| 84 | + # Go through the commands. |
| 85 | + for button, command in COMMANDS.items(): |
| 86 | + |
| 87 | + # Check if the command has a matching button. |
| 88 | + if button in pressed: |
| 89 | + |
| 90 | + # Now we can unpack the command. |
| 91 | + name, target, color = command |
| 92 | + |
| 93 | + # Execute command. |
| 94 | + print("Selected:", name) |
| 95 | + remote.light.on(color) |
| 96 | + switch_target(target) |
| 97 | + |
| 98 | + # Wait for the button to be released. |
| 99 | + while button in remote.buttons.pressed(): |
| 100 | + wait(10) |
| 101 | + |
| 102 | + # Activate function_motor motor based on the red buttons. |
| 103 | + if Button.LEFT in pressed: |
| 104 | + function_motor.dc(FUNCTION_POWER) |
| 105 | + elif Button.RIGHT in pressed: |
| 106 | + function_motor.dc(-FUNCTION_POWER) |
| 107 | + else: |
| 108 | + function_motor.stop() |
| 109 | + |
| 110 | + # Choose drive speed based on +/- buttons. |
| 111 | + left_speed = 0 |
| 112 | + right_speed = 0 |
| 113 | + if Button.LEFT_PLUS in pressed: |
| 114 | + left_speed += DRIVE_SPEED |
| 115 | + if Button.LEFT_MINUS in pressed: |
| 116 | + left_speed -= DRIVE_SPEED |
| 117 | + if Button.RIGHT_PLUS in pressed: |
| 118 | + right_speed += DRIVE_SPEED |
| 119 | + if Button.RIGHT_MINUS in pressed: |
| 120 | + right_speed -= DRIVE_SPEED |
| 121 | + |
| 122 | + # Activate the driving motors. |
| 123 | + left_motor.run(left_speed) |
| 124 | + right_motor.run(right_speed) |
0 commit comments