#LOWER 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)

left_track = Motor(Port.A)
right_track = Motor(Port.B)
turret = Motor(Port.D)

lSpeed = 0
rSpeed = 0


def trackMove(lSpeed, rSpeed):
    left_track.run(speed = lSpeed)
    right_track.run(speed = rSpeed)




while True:
    if keyboard.poll(0):


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

        if key in('4'):
            key1 = 1000
        elif key in('6'):
            key1 = -1000
        else:
            key1 = 0        




        turret.run(key1 * 1000)

        if key in('w'):
            lSpeed = 1000
            rSpeed = -1000
            
        elif key in('s'):
            lSpeed = -1000
            rSpeed = 1000

        elif key in('a'):
            lSpeed = -1000
            rSpeed = -1000

        elif key in('d'):
            lSpeed = 1000
            rSpeed = 1000

        elif key in('4'):
            key1 = 1
        elif key in('6'):
            key1 = -1    

        else:
            lSpeed = 0
            rSpeed = 0


        




        
        trackMove(lSpeed,rSpeed)