diff --git a/codes_for_scrimmage/hazmat/mainhazmatUPD.py b/codes_for_scrimmage/hazmat/mainhazmatUPD.py index c48de0b..cebadd0 100644 --- a/codes_for_scrimmage/hazmat/mainhazmatUPD.py +++ b/codes_for_scrimmage/hazmat/mainhazmatUPD.py @@ -140,35 +140,16 @@ async def Run4(): # From Send_Over_Final.py # Add Rishi's code here async def Run5(): - await drive_base.straight(519) - await left_arm.run_angle(-10000, 300) - await left_arm.run_angle(10000, 600) - await drive_base.straight(160) - await drive_base.turn(-30) - await drive_base.straight(50) - await right_arm.run_angle(3000, 3000) - await drive_base.straight(-150) - await drive_base.turn(120) - await drive_base.straight(25) - await right_arm.run_angle(10000, -3000) - await drive_base.straight(-110) - await drive_base.turn(-43) - await right_arm.run_angle(10000, -3000) - await drive_base.straight(295) - await right_arm.run_angle(10000, 9000) - await drive_base.straight(-65) - await drive_base.turn(45) - await drive_base.straight(175) - await drive_base.turn(24.5) + await drive_base.straight(600) await drive_base.straight(-100) - await right_arm.run_angle(10000, -8500) + await drive_base.straight(150) + await drive_base.turn(60) await drive_base.straight(100) - await right_arm.run_angle(10000, 3500) - await drive_base.turn(-30) - await drive_base.straight(-300) - await drive_base.turn(-80) - await drive_base.straight(-700) - + await drive_base.turn(-86) + await drive_base.straight(120) + await drive_base.turn(-45) + await drive_base.straight(-200) + await drive_base.turn(75) # Add - Adi's code here async def Run6(): diff --git a/missions/New_MapReveal/New_MapReveal_Mineshaft.py b/missions/New_MapReveal/New_MapReveal_Mineshaft.py new file mode 100644 index 0000000..2113b9b --- /dev/null +++ b/missions/New_MapReveal/New_MapReveal_Mineshaft.py @@ -0,0 +1,38 @@ +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) +left_arm = Motor(Port.C) +right_arm = Motor(Port.D) +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, 500, 300, 200) +drive_base.use_gyro(True) + +WALL_DISTANCE = 200 # mm + +async def main(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-220) + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -1200) + await drive_base.straight(84) + await right_arm.run_angle(300, 1200) + await drive_base.straight(-875) + +run_task(main()) \ No newline at end of file