From 4942f28e67c9f88faee75fc5d564500fdf653555 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Tue, 6 Jan 2026 22:23:43 +0000 Subject: [PATCH] Update competition_codes/state/sunprarie_state_main.py Battery diagnostics added. --- .../state/sunprarie_state_main.py | 91 +++++++++++++++---- 1 file changed, 71 insertions(+), 20 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 46d8021..fd889af 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/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) @@ -47,14 +68,18 @@ async def monitor_distance(): break 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 @@ -78,7 +104,7 @@ async def Run1(): #await drive_base.turn(-100) await drive_base.arc(200,None,-300) drive_base.stop() - + async def solve_whats_on_sale(): right_arm.run_angle(500,30) @@ -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) - #print(color) + 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()) \ No newline at end of file