#FOR UPPER HUB
from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor 
from pybricks.parameters import Port, Stop, Color
from pybricks.tools import wait

from usys import stdin
from uselect import poll

hub = TechnicHub()
keyboard = poll()
keyboard.register(stdin)


boom_motor = Motor(Port.A)
arm_motor = Motor(Port.B)
bucket_motor = Motor(Port.C)
scoop_motor = Motor(Port.D)

while True:
    if keyboard.poll(0):


        key = stdin.read(1)
        print("Key pressed", key)

        if key in ('w'):
            key1 = 1
        elif key in('s'):
            key1 = -1
        else:
            key1 = 0

        boom_motor.run(key1 * 1000)

        if key in('a'):
            key2 = 1
        elif key in('d'):
            key2 = -1
        else:
            key2 = 0        



        arm_motor.run(key2 * 1000)


        if key in('8'):
            key3 = 1
        elif key in('2'):
            key3 = -1
        else:
            key3 = 0        



        bucket_motor.run(key3 * 1000)


        if key in('4'):
            key4 = 1
        elif key in('6'):
            key4 = -1
        else:
            key4 = 0        




        scoop_motor.run(key4 * 1000)