diff --git a/discovery-phase/missions/mission_15_Parthiv.py b/discovery-phase/missions/mission_15_Parthiv.py new file mode 100644 index 0000000..2b5caed --- /dev/null +++ b/discovery-phase/missions/mission_15_Parthiv.py @@ -0,0 +1,53 @@ +#Solution for biosentric center +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(700, 500, 300, 200) +drive_base.use_gyro(True) + +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +async def run_both_arms(): + + await multitask( + left_arm.run_angle(200,80), + right_arm.run_angle(200,-100) + ) + +async def main (): + await left_arm.run_angle(200,-80) + await right_arm.run_angle(200,-70) + await drive_base.arc(-400,angle=90) + await drive_base.turn(55) + await drive_base.straight(240) + await drive_base.turn(-40) + await drive_base.straight(85) + await drive_base.straight(-32) + await run_both_arms() + await drive_base.straight(-53) + await right_arm.run_angle(200,100) + await drive_base.turn(30) + await drive_base.straight(-775) + await drive_base.stop + + +run_task(main())