from pybricks.hubs import TechnicHub
from pybricks.iodevices import XboxController
from pybricks.parameters import Axis, Button, Direction, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait

# Set up all devices.
#hub = TechnicHub(top_side=Axis.Z, front_side=Axis.X, broadcast_channel=5)
b1 = Motor(Port.A, Direction.CLOCKWISE)
b2 = Motor(Port.B, Direction.COUNTERCLOCKWISE)
jade = Motor(Port.D, Direction.CLOCKWISE) #jezdzenie
skret = Motor(Port.C, Direction.COUNTERCLOCKWISE) #skret
#drive_base = DriveBase(left, right, 48, 208)
xbox = XboxController()


#A B Bloom

# The main program starts here.
while True:
# If Y or X have a nonzero value:
# - Then use Y for speed and X for steering.
# - Otherwise stop.
#if xbox.joystick_left()[1] or xbox.joystick_left()[0]:
# drive_base.drive(xbox.joystick_left()[1] * 2, xbox.joystick_left()[0] * 2)
#else:
# drive_base.stop()
# We use the left minus the right trigger to control the turn table.
# This lets you rotate it by pressing one or the other.
# If that speed is zero, then coast the motor.

DEAD_ZONE = 0.05 # Definicja martwej strefy
trigger_difference = xbox.triggers()[0] - xbox.triggers()[1]

if abs(trigger_difference) > DEAD_ZONE:
# Skalowanie prędkości silnika
jade.run(trigger_difference * 30)
else:
jade.stop() # Zatrzymanie silnika

if abs(joystick_x) > DEAD_ZONE:
# Skalowanie i sterowanie
skret.run((joystick_x) * 3)
else:
skret.stop() # Zatrzymanie silnika

if Button.A in xbox.buttons.pressed():
b1.run(-1000) # Uruchom silnik w prawo z prędkością 100

elif Button.Y in xbox.buttons.pressed():
b1.run(1000) # Uruchom silnik w lewo z prędkością 100

else:
b1.stop() # Zatrzymaj silnik, gdy żaden z przycisków nie jest wciśnięty

if Button.X in xbox.buttons.pressed():
b2.run(-1000) # Uruchom silnik w prawo z prędkością 100

elif Button.B in xbox.buttons.pressed():
b2.run(1000) # Uruchom silnik w lewo z prędkością 100

else:
b2.stop() # Zatrzymaj silnik, gdy żaden z przycisków nie jest wciśnięty


# Operate boom using direction pad up/down.
#boom_speed = 100 if Button.UP in xbox.buttons.pressed() else (-100 if Button.DOWN in xbox.buttons.pressed() else 0)
# Operate jib using right joystick vertical movement.
#jib_speed = xbox.joystick_right()[1]
# Operate hoist using bumpers.
#hoist_speed = 100 if Button.LB in xbox.buttons.pressed() else (-100 if Button.RB in xbox.buttons.pressed() else 0)
# Broadcast the values, so the hub in the crane can observe it.
#hub.ble.broadcast([boom_speed, jib_speed, hoist_speed])


wait(50)