New project for Lego Technic Cat D11 bulldozer

[vandalon]

Hi,

I've created a project to control the Cat D11 bulldozer with keyboard to get aquatinted with pybricks. Since it works rather nicely I would like to share it with the community. Could you please add it to the project page?

the code (I used other examples from the pybrick page if some stuff seems familiar):

# Import required MicroPython libraries.
from usys import stdin
from uselect import poll
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Button, Color, Direction, Port
from pybricks.tools import wait, StopWatch
from pybricks.hubs import TechnicHub

# Register hub and set light to soft white (gray)
hub = TechnicHub()
hub.light.on(Color.GRAY)

# Register the standard input so we can read keyboard presses.
keyboard = poll()
keyboard.register(stdin)

# Set defaults
left_speed = 0
right_speed = 0
function = "Blade up/down"
functionID = 1
key = None
state = "Idle"

# Default speed and acceleration. You can change
# them to make it more realistic.
SWITCH_SPEED = 720
DRIVE_SPEED = 100
DRIVE_ACCELERATION = 1000
FUNCTION_POWER = 100
MAX_LOAD = 75

# Initialize the motors.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
function_motor = Motor(Port.C)
switch_motor = Motor(Port.D)

left_motor.control.limits(acceleration=DRIVE_ACCELERATION)
right_motor.control.limits(acceleration=DRIVE_ACCELERATION)

# Define Functions
COMMANDS = {
    1: ("Blade up/down", 0, Color.BLUE),
    2: ("Blade tilt", 270, Color.RED),
    3: ("Ladder", 180, Color.YELLOW),
    4: ("Ripper", 90, Color.GREEN),
}

# Find the right end left endstop positions
right_end = switch_motor.run_until_stalled(500, duty_limit=50)
left_end = switch_motor.run_until_stalled(-500, duty_limit=50)

# The full switch motion ranges 270 degrees. Everything beyond that
# is play towards the endstops, equal in both directions. Since we
# are currently at the left endpoint, we can reset the angle
# accordingly. This way, the functions # are simply at the
# relative positions 0, 90, 180, 270.
switch_motor.reset_angle(-(right_end - left_end - 270) / 2)


def switch_target(target_angle):
    # Try up to 5 times.
    for i in range(5):
        # Keep track of how long we've tried.
        watch = StopWatch()
        switch_motor.run_target(SWITCH_SPEED, target, wait=True)
        # Wait until the stopwatch times out.
        while watch.time() < 2000:
            # We're done if the motor is on target, so exit
            # this function.
            if switch_motor.control.done():
                switch_motor.stop()
                return
            wait(10)

        # Otherwise, we got stuck, so try wiggling around to
        # release it.
        print("Getting unstuck.")
        switch_motor.run_target(SWITCH_SPEED, 0, wait=False)
        wait(1500)
        switch_motor.run_target(SWITCH_SPEED, 270, wait=False)
        wait(1500)
    # Always turn off motor to safe power
    switch_motor.stop()

# Print usage information
print(
    "\n\n\nKeys (FWD/REV/STP):\tw/s/x: Left track, e/d/c: Right Track, r/f/v: Both tracks full speed,",
    "\n\t\t\tq/a/z: Function, 0: Full stop, !: Shutdown.",
    "\nFuntions:\t\t1: Blade up/down 2: Blade tilt, 3: Ladder, 4: Ripper\n\n")

inc=0
while True:
    wait(10)

    # Stop function motor when jammed or blocked
    if function_motor.load() > MAX_LOAD or function_motor.load() < -MAX_LOAD:
        function_motor.stop()

    # Print statistical information
    inc += 1
    if inc % 50 == 0:
        LINE_CLEAR = '\x1b[2K'
        print(LINE_CLEAR,
            " State", state, "| Left: ",left_motor.speed(), left_motor.load(), "[", left_speed, 
            "], Right: ", right_motor.speed(), right_motor.load(), "[", right_speed,
            "] | Func.:", function,
            "| Bat:", hub.battery.voltage(), hub.battery.current(), end="\r"
            )
        inc = 0
    
    # Turn of motors when state it stopped for more then 5 seconds
    if state == "Stopped":
        if stopInc > 500:
            right_motor.stop()
            left_motor.stop()
            state = "Idle"
        stopInc += 1
    else:
        stopInc = 0

    # Restart loop of no keyboard input is given
    if not keyboard.poll(0):
        continue

    # Register key press
    if keyboard.poll(0):
        key = stdin.read(1)

    # Full stop
    if key == "0":
        state = "Idle"
        right_speed = 0
        left_speed = 0
        right_motor.stop()
        left_motor.stop()
        function_motor.stop()
        switch_motor.stop()

    # Shutdown hub
    if key == '!':
        print(LINE_CLEAR, "Good Bye!")
        wait(100)
        hub.system.shutdown()

    if key == "1" or key == "2" or key == "3" or key == "4":
        # Stop function motor and switch motor
        function_motor.stop()
        switch_motor.stop()

        # Go through the commands.
        for button, command in COMMANDS.items():
            # Check if the command has a matching button.
            if int(key) == button:
                # Now we can unpack the command.
                name, target, color = command
                # Execute command.
                switch_target(target)
                # Set color
                hub.light.on(color)
                # Update status info
                function = name
                functionID = button

    # Full speed ahead
    if key == "r":
        right_speed = 1000
        left_speed = 1000
        state = "Running"
    # Full speed reverse
    if key == "f":
        right_speed = -1000
        left_speed = -1000
        state = "Running"
    # Full stop
    if key == "v":
        right_speed = 0
        left_speed = 0
    # Increase left track speed
    if key == "w":
        left_speed += DRIVE_SPEED
        if left_speed > 1000:
            left_speed = 1000
        state = "Running"
    # Decrase left track speed
    if key == "s":
        left_speed -= DRIVE_SPEED
        if left_speed < -1000:
            left_speed = -1000
        state = "Running"
    # Stop left track
    if key == "x":
        left_speed = 0
    # Increate right track speed
    if key == "e":
        right_speed += DRIVE_SPEED
        if right_speed > 1000:
            right_speed = 1000
        state = "Running"
    # Decrease right track speed
    if key == "d":
        right_speed -= DRIVE_SPEED
        if right_speed < -1000:
            right_speed = -1000
        state = "Running"
    # Stop right track
    if key == "c":
        right_speed = 0
    # Function motor up
    if key == "q":
        function_motor.dc(-FUNCTION_POWER)
        if functionID == 1:
            MAX_LOAD = 150
        else:
            MAX_LOAD = 75 
    # Function motor down
    if key == "a":
        function_motor.dc(FUNCTION_POWER)
        MAX_LOAD = 170
    # Stop function motor
    if key == "z":
        function_motor.stop()

    # Activate the driving motors.
    if state != "Idle":
        left_motor.run(left_speed)
        right_motor.run(right_speed)

    # Set state to idle when speed is 0 on both tracks
    if left_speed == 0 and right_speed == 0 and state == "Running":
        state = "Stopped"