Update competition_codes/state/sunprarie_state_main.py

Battery diagnostics added.

competition_codes/state/sunprarie_state_main.py

@@ -1,5 +1,10 @@
-#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
 
+Francisco Liwa
+7:58 AM (8 hours ago)
+to me
+
+#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
+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
@@ -23,8 +28,24 @@ 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
+"""
+Debugging helps
+"""
+DEBUG = 0  # Enable when you want to show logs
+# Example conversion function (adjust min/max values as needed for your hub)
+async def get_battery_percentage(voltage_mV:float):
+    max_voltage = 8400.0  # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
+    min_voltage = 5000.0  # min battery level
+    percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
+    return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
 
+async def wait_button_release():
+    """Wait for all buttons to be released"""
+    while hub.buttons.pressed():
+        await wait(500)
+    await wait(1000)  # Debounce delay
+
+WALL_DISTANCE = 200  # mm
 async def drive_forward():
     """Drive forward continuously using DriveBase."""
     drive_base.drive(1000,0)
@@ -48,13 +69,17 @@ async def monitor_distance():
         if distance is None:
             continue
        
-        
         # Small delay to prevent overwhelming the sensor
         await wait(50)
 
-
+# Use this to set default
 def set_default_speed():
     drive_base.settings(600, 500, 300, 200)
+
+# Use this to change drive base movement
+def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
+    drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)  
+
 """
 Run#1
 - Removed forge and who lived here part
@@ -69,8 +94,9 @@ async def Run1():
     # Gentle stall detection (shorter distance = faster)
     await left_arm.run_until_stalled(1500, duty_limit=15)
     left_arm.reset_angle(0)
+    print(f"Initial left arm angle : {left_arm.angle()}")
 
-    await solve_whats_on_sale()
+    await solve_whats_on_sale_v2()
     await solve_silo()
  
     # return to base
@@ -86,6 +112,33 @@ async def solve_whats_on_sale():
     #Automated inconsistency
     #left_arm.run_angle(500,-119.5)
     left_arm.run_target(500,-121.5, Stop.HOLD)
+    print(f"Position left arm angle : {left_arm.angle()}")
+    
+    await drive_base.straight(180)
+
+    await drive_base.turn(-40)
+    await drive_base.straight(335)
+    await left_arm.run_angle(500,-20)
+
+    await drive_base.straight(-100)
+    await drive_base.straight(60)
+    await left_arm.run_angle(500,50)
+
+    await drive_base.straight(-100)
+    left_arm.run_angle(500,-50)
+    await drive_base.turn(-20)
+    left_arm.run_angle(1000,180)
+    await drive_base.turn(15)
+
+async def solve_whats_on_sale_v2():
+
+    right_arm.run_angle(500,30)
+
+   # Bring down left arm to position
+    await left_arm.run_angle(2000, -120)
+    #await left_arm.run_until_stalled(-500,duty_limit=15)
+    print(f"Position left arm angle : {left_arm.angle()}")
+    left_arm.reset_angle(0)
     
     await drive_base.straight(180)
 
@@ -109,12 +162,14 @@ async def solve_silo():
     await drive_base.straight(95)
 
     SPEED = 10000 # speed in degree per second
-    SWING_ANGLE = 80 # the angle!
+    SWING_ANGLE = 60 # the angle!
+    REBOUND_ADJ = 20
 
     # 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(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up
+        await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down
+       
 
     right_arm.run_angle(4000,60, Stop.HOLD)
 
@@ -430,12 +485,15 @@ def detect_color(h, s, v, reflected):
 
 async def main():
     while True:
+        pressed =  hub.buttons.pressed()
         h, s, v = await color_sensor.hsv()
-        #print(color_sensor.color())
-        #print(h,s,v)
         reflected = await color_sensor.reflection()
         color = detect_color(h, s, v, reflected)
+        if DEBUG :
+            #print(color_sensor.color())
+            #print(h,s,v)
             #print(color)
+            print(f"button pressed: {pressed}")
         
 
         if color == "Green":
@@ -458,10 +516,3 @@ async def main():
             await Run11()  
         elif color == "Light_Blue":
             print("Running Mission 2_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())