From 5a5b6d2d16d2eedff3c6709ccecc720cf4eb798e Mon Sep 17 00:00:00 2001
From: Johannes <31liwaj@elmbrookstudents.org>
Date: Wed, 24 Dec 2025 16:34:38 +0000
Subject: [PATCH] Update competition_codes/state/sunprarie_state_main.py

Code optimization and additions to 1st run.
---
 .../state/sunprarie_state_main.py             | 41 +++++++++++++------
 1 file changed, 29 insertions(+), 12 deletions(-)

diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py
index 942e488..29f4509 100644
--- a/competition_codes/state/sunprarie_state_main.py
+++ b/competition_codes/state/sunprarie_state_main.py
@@ -50,6 +50,9 @@ async def monitor_distance():
         # Small delay to prevent overwhelming the sensor
         await wait(50)
 
+
+def set_default_speed():
+    drive_base.settings(600, 500, 300, 200)
 """
 Run#1
 - Removed forge and who lived here part
@@ -58,11 +61,16 @@ Run#1
 """
 async def Run1():  
     
-    await left_arm.run_until_stalled(1500 ,duty_limit=15)
+    # Fast approach to near-stall position
+    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)
+
     await solve_whats_on_sale()
     await solve_silo()
-
+ 
     # return to base
     await drive_base.straight(-90)
     #await drive_base.turn(-100)
@@ -94,14 +102,14 @@ async def solve_silo():
     await drive_base.turn(42)
     await drive_base.straight(90)
 
+    SPEED = 10000 # speed in degree per second
+    SWING_ANGLE = 80 # the angle!
+
+    # Repeat this motion 4 times
+    for _ in range(4):
+        await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.COAST) # Swing up
+        await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.COAST) # Swing down
 
-    await right_arm.run_angle(10000,-80, Stop.HOLD)
-    await right_arm.run_angle(10000,80, Stop.HOLD)
-    await right_arm.run_angle(10000,-80, Stop.HOLD)
-    await right_arm.run_angle(10000,80, Stop.HOLD)
-    await right_arm.run_angle(10000,-80, Stop.HOLD)
-    await right_arm.run_angle(10000,80, Stop.HOLD)
-    await right_arm.run_angle(10000,-80, Stop.HOLD)
     right_arm.run_angle(4000,60, Stop.HOLD)
 
 
@@ -319,7 +327,16 @@ 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
+    
+    await drive_base.straight(680)
+    drive_base.settings(150, 750, 50, 750)
+    await drive_base.turn(-45)
+    await drive_base.straight(200)
+    set_default_speed()
+    await drive_base.straight(-100)
+    await drive_base.turn(45)
+    drive_base.stop()
 
 # Function to classify color based on HSV
 def detect_color(h, s, v, reflected):
@@ -371,7 +388,7 @@ async def main():
             await Run5() 
         elif color == "Purple":
             print('Running Mission 6')
-            await Run6_7()  
+            await Run10()  
         elif color == "Light_Blue":
             print("Running Mission 2_1")
             await Run2_1()
@@ -379,4 +396,4 @@ async def main():
             print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
         await wait(10)
 # Run the main function
-run_task(main())
+run_task(main())
\ No newline at end of file