from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks.tools import multitask, run_task

# Initialize hub and devices
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)

# DriveBase configuration
WHEEL_DIAMETER = 62.4  # mm (adjust for your wheels)
AXLE_TRACK = 150  # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)

async def set_default_speed():
    drive_base.settings(600, 500, 300, 200)

async def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
    drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
async def main():

    await drive_base.straight(850)
    await drive_base.turn(-20)
    await drive_base.straight(100)
    await drive_base.straight(-30)
    await drive_base.turn(-50)
    await drive_base.turn(60)
    await drive_base.straight(-200)
    await drive_base.turn(15)
    await drive_base.straight(-800)
run_task(main())