competition_codes/state/sunprarie_state_main.py

@@ -323,7 +323,26 @@ async def solve_site_mark_2():
     await wait(50)
     await right_arm.run_angle(50, 50)
 
-async def Run10():  # experiment with ferris wheel for Site Markings
+async def Run10():  # experimental map reveal attachment
+    
+    await drive_base.straight(600)
+    drive_base.settings(150, 750, 50, 500)
+    await drive_base.turn(-30)
+    await drive_base.straight(260)
+    left_arm.run_angle(300,218)
+    
+    set_default_speed()
+    await drive_base.straight(-80)
+    await drive_base.turn(30)
+    await drive_base.straight(-300)
+    await drive_base.straight(400)
+    #await left_arm.run_angle(50,120)
+    await drive_base.straight(-200)
+    await left_arm.run_angle(300,-215)
+    await drive_base.straight(-600)
+    drive_base.stop()
+
+async def Run11():  # experimental surface brushing attachment
     
     await drive_base.straight(600)
     drive_base.settings(150, 750, 50, 500)
@@ -342,6 +361,46 @@ async def Run10():  # experiment with ferris wheel for Site Markings
     await drive_base.straight(-600)
     drive_base.stop()
 
+async def Run12():  # experimental careful recovery attachment
+    
+    right_arm.reset_angle(0)
+    # This raises the left arm to avoid entanglement when turning
+    await left_arm.run_angle(2000, 180)  # Fast movement upward
+    # Gentle stall detection (shorter distance = faster)
+    await left_arm.run_until_stalled(1500, duty_limit=15)
+    left_arm.reset_angle(0)
+    
+    # Drive the robot to the wall 
+    await drive_base.straight(900)
+    await drive_base.turn(83) # Align robot to Mineshaft entrance
+
+    # This change robot movement to slow
+    drive_base.settings(100, 100, 50, 500)
+
+    # Bring down left arm to position
+    await left_arm.run_angle(2000, -180)
+    await left_arm.run_until_stalled(-1500,duty_limit=15)
+    left_arm.reset_angle(0)
+
+    # Bring down right arm to position
+    await right_arm.run_target(2000,-120)
+    right_arm.reset_angle(0)
+    
+    await drive_base.straight(190) # Slowly move straight to mineshaft 190 mm
+
+    await right_arm.run_angle(100,95,Stop.HOLD) # Raise mineshaft
+    await wait(50)
+    await left_arm.run_angle(100,60) # recover artifact by lifting arm 60 degrees
+
+    # Moving back
+    await drive_base.straight(-200)
+    
+    # Return home
+    set_default_speed()   # change movement robot movement to fast
+    await drive_base.turn(100)
+    await drive_base.straight(700)
+    drive_base.stop()
+
 # Function to classify color based on HSV
 def detect_color(h, s, v, reflected):
     if reflected > 4:
@@ -368,11 +427,11 @@ def detect_color(h, s, v, reflected):
 async def main():
     while True:
         h, s, v = await color_sensor.hsv()
-        print(color_sensor.color())
-        print(h,s,v)
+        #print(color_sensor.color())
+        #print(h,s,v)
         reflected = await color_sensor.reflection()
         color = detect_color(h, s, v, reflected)
-        print(color)
+        #print(color)
         
 
         if color == "Green":
@@ -392,12 +451,13 @@ async def main():
             await Run5() 
         elif color == "Purple":
             print('Running Mission 6')
-            await Run10()  
+            await Run11()  
         elif color == "Light_Blue":
             print("Running Mission 2_1")
-            await Run2_1()
+            await Run12()
         else:
             print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
+            #pass
         await wait(10)
 # Run the main function
 run_task(main())