codes_for_scrimmage/hazmat/hazmat_main.py

@@ -232,19 +232,22 @@ async def Run1(): #From M8_5.py
     await drive_base.straight(-600)
 
 async def Run2(): #From Heavy_lifting_final.py
+     #Get to mission
     await drive_base.straight(200)
     await drive_base.turn(-20)
-    await drive_base.straight(536)
-    await drive_base.turn(60, Stop.HOLD)
-    await drive_base.straight(30)
-
-    await right_arm.run_angle(5000,2900)
-    await drive_base.straight(40)
-    await right_arm.run_angle(5000,-4000)
-    await drive_base.straight(-60)
-
-    await drive_base.turn(-60)
-    await drive_base.straight(-670)
+    await drive_base.straight(525)
+    await drive_base.turn(60)
+    #Solve mission
+    await drive_base.straight(50)
+    await right_arm.run_angle(2000,1000)
+    await drive_base.straight(-50)
+    await drive_base.turn(45)
+    await drive_base.straight(50)
+    await right_arm.run_angle(350,-1000)
+    #Return home
+    await drive_base.straight(-100)
+    await drive_base.turn(-100)
+    await drive_base.straight(-600)
 
 
 async def Run3(): #tip the scale.py
@@ -304,11 +307,39 @@ async def Run4(): #From Send_Over_Final.py
 
 #Add  Rishi's code here
 async def Run5(): 
-    pass
+    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(135)
+    await drive_base.straight(25)
+    await right_arm.run_angle(10000, -3000)
+    await drive_base.straight(-100)
+    await drive_base.turn(-55)
+    await right_arm.run_angle(10000, -3000)
+    await drive_base.straight(250)
+    await drive_base.turn(-5)
+    await right_arm.run_angle(10000, 7000)
+    await drive_base.straight(-50)
+    await drive_base.turn(45)
+    await drive_base.straight(100)
+    await drive_base.turn(37)
+    await right_arm.run_angle(10000, -6000)
+    await drive_base.straight(90)
+    await right_arm.run_angle(3000, 3000)
+    await drive_base.turn(-40)
 
 #Add - Adi's code here
 async def Run6():
-   pass
+    await drive_base.straight(420)
+    await right_arm.run_angle(300,-100)
+    await drive_base.straight(-100)
+    await right_arm.run_angle(300, 100)
+    await drive_base.straight(-350)
 
 async def main():
     while True: