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

# 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 = 68.8  # mm (adjust for your wheels)
AXLE_TRACK = 180  # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 450, 300, 200)
drive_base.use_gyro(True)

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

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

#Run code
drive_base.straight(50)#Was 50
drive_base.turn(-49)#42 Mostly works
drive_base.straight(700)#Was 735
#left_arm.run_angle(1500,-100)#Was 1000,And was -30
#wait(1000)
#drive_base.straight(330)
drive_base.turn(40)#Was 80
set_speed(100,300,300,200)
drive_base.straight(100)
drive_base.turn(47)#Was 37
drive_base.straight(320)#Was 300-350
drive_base.straight(-110)#Was -125
set_default_speed
drive_base.turn(85)#Was 87
#right_arm.run_angle(1000, -30)#Was -47
drive_base.straight(50)#150
drive_base.turn(-67)#Was -73
print("Mycellium")
left_arm.run_angle(1000,180)#Towards Mycellium
drive_base.straight(75)#Was 75
drive_base.turn(-10)#Was Positive 30
drive_base.straight(3)
set_speed(1000,300,300,200)
left_arm.run_angle(102020,-180)
drive_base.turn(70)#Orignal 120
drive_base.straight(300)