Hello World - Code

From RoboWiki
Jump to: navigation, search

Return back to project page: Hello World - Jakub Vojtek

Python code for the Hello World project:

 
from buildhat import MotorPair, ColorSensor, DistanceSensor
from pynput import keyboard as kb
import time

wheels = MotorPair('A', 'B')
distanceSensor = DistanceSensor('C')
colorSensor = ColorSensor('D')

key_state = {
    "left": False,
    "right": False,
    "up": False,
    "down": False
}

last_direction = ""
running = True

def update_key_state(key, state):
    if key in key_state:
        key_state[key] = state

def get_current_direction():
    if key_state["up"]:
        if key_state["left"]:
            return "up_left"
        elif key_state["right"]:
            return "up_right"
        else:
            return "up"
    elif key_state["down"]:
        if key_state["left"]:
            return "down_left"
        elif key_state["right"]:
            return "down_right"
        else:
            return "down"
    elif key_state["left"]:
        return "left"
    elif key_state["right"]:
        return "right"
    else:
        return "none"

def on_press(key):
    try:
        if key == kb.Key.space:
            color = colorSensor.get_color()
            print(f"Detected color: {color}")
    except AttributeError:
        pass

    if key == kb.Key.left:
        update_key_state("left", True)
    elif key == kb.Key.right:
        update_key_state("right", True)
    elif key == kb.Key.up:
        update_key_state("up", True)
    elif key == kb.Key.down:
        update_key_state("down", True)

def on_release(key):
    global running
    if key == kb.Key.esc:
        running = False
        return False

    if key == kb.Key.left:
        update_key_state("left", False)
    elif key == kb.Key.right:
        update_key_state("right", False)
    elif key == kb.Key.up:
        update_key_state("up", False)
    elif key == kb.Key.down:
        update_key_state("down", False)

def check_reverse():
    distance = distanceSensor.get_distance()
    if distance != -1 and distance < 200 and get_current_direction() in ["down", "down_left", "down_right"]:
        wheels.stop()
        print("Obstacle detected! Stopping the wheels.")
    time.sleep(0.1)

def main():
    global last_direction
    print("Press arrow keys for movement and spacebar for color detection. Press 'esc' to exit.")

    listener = kb.Listener(on_press=on_press, on_release=on_release)
    listener.start()

    while running:
        current_direction = get_current_direction()
        check_reverse()

        if current_direction != last_direction:
            if current_direction == "up":
                wheels.start(-50, 50)
            elif current_direction == "down":
                wheels.start(50, -50)
            elif current_direction == "left":
                wheels.start(-20, -20)
            elif current_direction == "right":
                wheels.start(20, 20)
            elif current_direction == "up_left":
                wheels.start(-20, 50)
            elif current_direction == "up_right":
                wheels.start(-50, 20)
            elif current_direction == "down_left":
                wheels.start(20, -50)
            elif current_direction == "down_right":
                wheels.start(50, -20)
            elif current_direction == "none":
                wheels.stop()

            last_direction = current_direction

        time.sleep(0.1)

    listener.stop()
    wheels.stop()
    colorSensor.off()
    distanceSensor.off()


if __name__ == '__main__':
    main()