diff --git a/README.md b/README.md index 3bb5d68..86d54dc 100644 --- a/README.md +++ b/README.md @@ -63,17 +63,14 @@ Repository ### Installation & Deployment - from the server - everyday -1. Download the file codes_for_scrimmage/regional-final/Final_combined.py +1. Download the file competition_codes/sectionals/sectional_main_dec_6.py - You can do this through the repo, by using cURL, or by using git. - - Repo - Go to [codes_for_scrimmage/regional-final/Final_combined.py](codes_for_scrimmage/regional-final/Final_combined.py) and click the "Download" button. - - cURL or another HTTP data transferrer - - - ```curl -o Final_combined.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/codes_for_scrimmage/regional-final/Final_combined.py``` + - Repo - Go to the [latest release](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/releases) and click the "Download as ZIP" button to get the full repository. + - Single file - Go to the latest release and click the file link to get the raw master file. - git CLI - - - ```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/codes_for_scrimmage/regional-final``` + ```git clone https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git``` - Then use Final_combined.py. + Then use the master file. 2. Open https://code.pybricks.com/ and select the "Import a file" button on the top of the left bar. Import the .py file. Pair your robot via Bluetooth in Pybricks by selecting the Bluetooth button. diff --git a/RoshoDaGoat.py b/RoshoDaGoat.py deleted file mode 100644 index 7d84ee2..0000000 --- a/RoshoDaGoat.py +++ /dev/null @@ -1,42 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch, run_task, multitask - -hub = PrimeHub() - -# Initialize both motors. In this example, the motor on the -# left must turn counterclockwise to make the robot go forward. -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) - -arm_motor = Motor(Port.D, Direction.CLOCKWISE) -arm_motor_left= Motor(Port.C, Direction.CLOCKWISE) -# Initialize the drive base. In this example, the wheel diameter is 56mm. -# The distance between the two wheel-ground contact points is 112mm. -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180) - -print('The default settings are: ' + str(drive_base.settings())) -drive_base.settings(300,1000,300,750) -# Optionally, uncomment the line below to use the gyro for improved accuracy. -drive_base.use_gyro(True) - -async def main(): - await drive_base.straight(519) - await arm_motor_left.run_angle(-10000, 300) - await arm_motor_left.run_angle(10000, 600) - await drive_base.straight(160) - await drive_base.turn(-30) - await drive_base.straight(50) - await arm_motor.run_angle(3000, 3000) - await drive_base.straight(-150) - await drive_base.turn(135) - await drive_base.straight(50) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(-100) - await drive_base.turn(-54) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(200) - await arm_motor.run_angle(10000, 10000) -run_task(main()) \ No newline at end of file diff --git a/competition_codes/AROC/Nationals_Main.py b/competition_codes/AROC/Nationals_Main.py new file mode 100644 index 0000000..000d55b --- /dev/null +++ b/competition_codes/AROC/Nationals_Main.py @@ -0,0 +1,534 @@ +#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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction +right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(1000, 600, 500, 300) +drive_base.use_gyro(True) + +""" +Debugging helps +""" +DEBUG = 1 # 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 = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,0) + +async def drive_backward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + drive_base.stop() + print(f"Wall detected at {distance}mm!") + 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 +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -210) # Fast movement downward + + # 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_v3() + await solve_silo() + + # return to base + await drive_base.straight(-90) + #await drive_base.turn(-100) + await drive_base.arc(200,None,-300) + drive_base.stop() + +async def solve_whats_on_sale_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(210) + + await drive_base.turn(-40) + await drive_base.straight(320) + await left_arm.run_angle(490,-30) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(600,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(5) + await drive_base.turn(-25) + left_arm.run_angle(5000,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(175) + + 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(-90) + left_arm.run_angle(500,-50) + left_arm.run_angle(1000,6) + await drive_base.turn(-20) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-75) + await drive_base.turn(45) + await drive_base.straight(110) + + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 + + # Repeat this motion 3 times + for _ in range(4): + 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) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + await solve_flag() + + # return to base + await drive_base.turn(50) + await drive_base.straight(-220) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100,90) + await right_arm.run_target(50,50) + await wait(800) + # await right_arm.run_angle(50,-30) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(95) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-23 ) + await drive_base.straight(-100) + await drive_base.turn(-3) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250,-90) + await left_arm.run_angle(250,90) + + +""" +Run#2.1 +- Alternate solution for Forge, Who lived here and Heavy Lifting combined +- Light Blue Key +- Different alignment +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50,50) + await right_arm.run_angle(50,-30) + await drive_base.straight(700) + # await drive_base.turn(-30) + # await drive_base.straight(20) + await drive_base.turn(-40) + await drive_base.straight(-30) + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_tip_the_scale() + await solve_angler_artifacts() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-270,None,365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000,-800) + await drive_base.straight(-130) + await drive_base.turn(110) + await drive_base.turn(-25) + + + + + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" +async def Run5(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + await drive_base.straight(300) + await drive_base.straight(-200) + await drive_base.turn(-15) + # Solving statue + await drive_base.straight(350) + await drive_base.turn(-104) + await drive_base.straight(-80) + await left_arm.run_angle(500, -300) + await drive_base.straight(120) + await drive_base.turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive_base.turn(18) + await drive_base.straight(-100) + await drive_base.turn(-90) + await drive_base.straight(900) + drive_base.stop() + + +async def solve_salvage_operation(): + await drive_base.straight(500) + await right_arm.run_angle(300,500) + await drive_base.straight(-75) + await right_arm.run_angle(300, -900) + await drive_base.straight(-350) + await wait(1000) + await drive_base.straight(800) + await drive_base.straight(-200) + await drive_base.turn(-15) + await drive_base.straight(350) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700,200) + await drive_base.turn(30) + +""" +Run#6 +- Solve 2/3 Site Markings +- Run only if have time +- Purple Key +""" +async def Run6_7(): # experiment with ferris wheel for Site Markings + solve_site_mark_1() + solve_site_mark_2() + #return to base + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + +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) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-100) + await drive_base.turn(30) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await drive_base.straight(-600) + drive_base.stop() + +async def Run12(): + await drive_base.straight(900) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Light_Blue" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + 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": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + 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 + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/competition_codes/AROC/The_South-Awakens.py b/competition_codes/AROC/The_South-Awakens.py new file mode 100644 index 0000000..fba12b4 --- /dev/null +++ b/competition_codes/AROC/The_South-Awakens.py @@ -0,0 +1,1017 @@ +#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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B, Direction.CLOCKWISE) +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12, 36]], [[12, 20, 24]]) +right_arm = Motor(Port.D, Direction.CLOCKWISE, [[12, 36], [12, 20, 24]]) +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm +AXLE_TRACK = 180 # mm +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +# ============================================================ +# ADVANCED PID MODULE +# ============================================================ + +class PIDController: + """ + Advanced PID Controller with: + - Anti-windup (integral clamping) + - Derivative filtering (low-pass) + - Deadband tolerance + - Output saturation + - Setpoint ramping + - Error history tracking for diagnostics + """ + + def __init__( + self, + kp: float, + ki: float, + kd: float, + setpoint: float = 0.0, + output_min: float = -100.0, + output_max: float = 100.0, + integral_limit: float = 1000.0, + deadband: float = 0.5, + derivative_filter: float = 0.2, + name: str = "PID", + ): + self.kp = kp + self.ki = ki + self.kd = kd + self.setpoint = setpoint + self.output_min = output_min + self.output_max = output_max + self.integral_limit = integral_limit + self.deadband = deadband + self.derivative_filter = derivative_filter + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._timer = StopWatch() + self._first_update = True + self.name = name + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + self._max_history = 50 + + def reset(self): + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._first_update = True + self._timer.reset() + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + + def set_setpoint(self, setpoint: float): + self.setpoint = setpoint + + def set_gains(self, kp: float, ki: float, kd: float): + self.kp = kp + self.ki = ki + self.kd = kd + + def _clamp(self, value: float, low: float, high: float) -> float: + if value < low: + return low + if value > high: + return high + return value + + def update(self, current_value: float) -> float: + dt_ms = self._timer.time() + self._timer.reset() + + if self._first_update or dt_ms <= 0: + dt = 0.02 + self._first_update = False + else: + dt = dt_ms / 1000.0 + + error = self.setpoint - current_value + + if abs(error) < self.deadband: + error = 0.0 + + p_term = self.kp * error + + self._integral += error * dt + self._integral = self._clamp( + self._integral, -self.integral_limit, self.integral_limit + ) + i_term = self.ki * self._integral + + if dt > 0: + raw_derivative = (error - self._prev_error) / dt + else: + raw_derivative = 0.0 + + alpha = self.derivative_filter + filtered_derivative = ( + alpha * self._prev_derivative + (1.0 - alpha) * raw_derivative + ) + d_term = self.kd * filtered_derivative + + output = p_term + i_term + d_term + output = self._clamp(output, self.output_min, self.output_max) + + self._prev_error = error + self._prev_derivative = filtered_derivative + self._last_output = output + self._last_p = p_term + self._last_i = i_term + self._last_d = d_term + + if len(self._error_history) >= self._max_history: + self._error_history.pop(0) + self._error_history.append(error) + + return output + + def is_settled(self, tolerance: float = 1.0, samples: int = 5) -> bool: + if len(self._error_history) < samples: + return False + recent = self._error_history[-samples:] + return all(abs(e) < tolerance for e in recent) + + def diagnostics(self) -> str: + return ( + f"[{self.name}] P={self._last_p:.2f} I={self._last_i:.2f} " + f"D={self._last_d:.2f} Out={self._last_output:.2f} " + f"Err={self._prev_error:.2f}" + ) + + +# ============================================================ +# PID-BASED DRIVE VARIABLE +# ============================================================ + +class PIDDrive: + """ + High-level PID driving interface that wraps the gyro and motors. + """ + + def __init__( + self, + hub_ref, + left: Motor, + right: Motor, + drivebase: DriveBase, + ultrasonic: UltrasonicSensor, + wheel_diameter: float = 68.8, + axle_track: float = 180.0, + ): + self.hub = hub_ref + self.left = left + self.right = right + self.db = drivebase + self.ultrasonic = ultrasonic + self.wheel_diameter = wheel_diameter + self.axle_track = axle_track + self.mm_per_degree = (umath.pi * wheel_diameter) / 360.0 + + self.straight_pid = PIDController( + kp=3.0, ki=0.05, kd=1.5, + output_min=-150, output_max=150, + integral_limit=500, deadband=0.3, + derivative_filter=0.3, name="Straight", + ) + + self.turn_pid = PIDController( + kp=4.0, ki=0.1, kd=2.0, + output_min=-400, output_max=400, + integral_limit=800, deadband=0.5, + derivative_filter=0.25, name="Turn", + ) + + self.distance_pid = PIDController( + kp=1.5, ki=0.02, kd=1.0, + output_min=-300, output_max=300, + integral_limit=500, deadband=2.0, + derivative_filter=0.3, name="Distance", + ) + + self.accel_rate = 5 + self.decel_distance = 80 + + def _get_heading(self) -> float: + return self.hub.imu.heading() + + def _reset_heading(self): + self.hub.imu.reset_heading(0) + + def _reset_motors(self): + self.left.reset_angle(0) + self.right.reset_angle(0) + + def _get_distance_traveled(self) -> float: + left_deg = abs(self.left.angle()) + right_deg = abs(self.right.angle()) + avg_deg = (left_deg + right_deg) / 2.0 + return avg_deg * self.mm_per_degree + + def _drive_motors(self, left_speed: float, right_speed: float): + self.left.run(left_speed) + self.right.run(right_speed) + + def _stop_motors(self, stop_type=Stop.BRAKE): + self.left.stop() + self.right.stop() + if stop_type == Stop.HOLD: + self.left.hold() + self.right.hold() + + async def pid_straight( + self, + distance_mm: float, + base_speed: float = 400, + settle_tolerance: float = 2.0, + settle_samples: int = 3, + timeout_ms: int = 8000, + ): + self._reset_heading() + self._reset_motors() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + direction = 1 if distance_mm >= 0 else -1 + target_dist = abs(distance_mm) + timer = StopWatch() + + while True: + traveled = self._get_distance_traveled() + remaining = target_dist - traveled + + if remaining <= 0: + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Straight] Timeout after {timeout_ms}ms") + break + + ramp_up_speed = min(base_speed, 100 + traveled * self.accel_rate) + if remaining < self.decel_distance: + decel_speed = max(80, base_speed * (remaining / self.decel_distance)) + else: + decel_speed = base_speed + speed = min(ramp_up_speed, decel_speed) + + current_heading = self._get_heading() + correction = self.straight_pid.update(current_heading) + + left_speed = direction * speed + correction + right_speed = direction * speed - correction + + self._drive_motors(left_speed, right_speed) + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Straight] dist={traveled:.0f}/{target_dist:.0f} " + f"spd={speed:.0f} {self.straight_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + if DEBUG: + print( + f"[PID Straight] Complete: {self._get_distance_traveled():.1f}mm " + f"of {distance_mm}mm" + ) + + async def pid_turn( + self, + angle_deg: float, + max_speed: float = 300, + settle_tolerance: float = 1.0, + settle_samples: int = 5, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.turn_pid.reset() + self.turn_pid.set_setpoint(angle_deg) + + timer = StopWatch() + + while True: + current_heading = self._get_heading() + output = self.turn_pid.update(current_heading) + + if output > max_speed: + output = max_speed + elif output < -max_speed: + output = -max_speed + + if abs(output) < 20 and abs(angle_deg - current_heading) > settle_tolerance: + if output > 0: + output = 20 + elif output < 0: + output = -20 + + self._drive_motors(output, -output) + + if self.turn_pid.is_settled(settle_tolerance, settle_samples): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Turn] Timeout after {timeout_ms}ms") + break + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Turn] heading={current_heading:.1f}/{angle_deg:.1f} " + f"{self.turn_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + final = self._get_heading() + print( + f"[PID Turn] Complete: {final:.1f}° of {angle_deg:.1f}° " + f"(error={angle_deg - final:.2f}°)" + ) + + async def pid_drive_until( + self, + base_speed: float = 300, + stop_distance_mm: float = 100, + timeout_ms: int = 10000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is not None and distance < stop_distance_mm: + if DEBUG: + print(f"[PID DriveUntil] Object at {distance}mm — stopping") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID DriveUntil] Timeout") + break + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + if distance is not None and distance < stop_distance_mm * 2: + speed_factor = distance / (stop_distance_mm * 2) + speed = max(80, base_speed * speed_factor) + else: + speed = base_speed + + self._drive_motors(speed + correction, speed - correction) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + + async def pid_align_to_wall( + self, + target_distance_mm: float = 100, + base_speed: float = 200, + tolerance: float = 3.0, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + self.distance_pid.reset() + self.distance_pid.set_setpoint(target_distance_mm) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is None: + await wait(20) + continue + + speed = -self.distance_pid.update(distance) + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + self._drive_motors(speed + correction, speed - correction) + + if self.distance_pid.is_settled(tolerance, 5): + if DEBUG: + print(f"[PID Align] Settled at {distance}mm") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Align] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + + async def pid_arc( + self, + radius_mm: float, + angle_deg: float, + base_speed: float = 300, + timeout_ms: int = 8000, + ): + self._reset_heading() + self.turn_pid.reset() + self._reset_motors() + + if radius_mm == 0: + await self.pid_turn(angle_deg, base_speed) + return + + abs_radius = abs(radius_mm) + outer_radius = abs_radius + (self.axle_track / 2.0) + inner_radius = abs_radius - (self.axle_track / 2.0) + + if inner_radius < 0: + inner_radius = 0 + + speed_ratio = inner_radius / outer_radius if outer_radius > 0 else 0 + + target_heading = angle_deg + self.turn_pid.set_setpoint(target_heading) + + direction = 1 if angle_deg >= 0 else -1 + timer = StopWatch() + + while True: + heading = self._get_heading() + correction = self.turn_pid.update(heading) + + if abs(target_heading) > 0: + progress = abs(heading) / abs(target_heading) + else: + progress = 1.0 + + if progress > 0.8: + speed = max(60, base_speed * (1.0 - progress) * 5) + else: + speed = base_speed + + outer_speed = direction * speed + inner_speed = direction * speed * speed_ratio + + if radius_mm > 0: + left_speed = outer_speed + correction * 0.3 + right_speed = inner_speed - correction * 0.3 + else: + left_speed = inner_speed + correction * 0.3 + right_speed = outer_speed - correction * 0.3 + + self._drive_motors(left_speed, right_speed) + + if self.turn_pid.is_settled(1.5, 4): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Arc] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + print(f"[PID Arc] Final heading: {self._get_heading():.1f}° / {angle_deg}°") + + +# ============================================================ +# INSTANTIATE THE PID DRIVE VARIABLE +# ============================================================ + +drive = PIDDrive( + hub_ref=hub, + left=left_motor, + right=right_motor, + drivebase=drive_base, + ultrasonic=lazer_ranger, + wheel_diameter=WHEEL_DIAMETER, + axle_track=AXLE_TRACK, +) + +# ============================================================ +# TUNING PRESETS +# ============================================================ + +def set_precise_mode(): + drive.straight_pid.set_gains(kp=4.0, ki=0.08, kd=2.0) + drive.turn_pid.set_gains(kp=5.0, ki=0.15, kd=2.5) + drive.accel_rate = 3 + drive.decel_distance = 100 + +def set_fast_mode(): + drive.straight_pid.set_gains(kp=2.5, ki=0.03, kd=1.0) + drive.turn_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.accel_rate = 8 + drive.decel_distance = 60 + +def set_default_pid_mode(): + drive.straight_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.turn_pid.set_gains(kp=4.0, ki=0.1, kd=2.0) + drive.accel_rate = 5 + drive.decel_distance = 80 + + +""" +Debugging helps +""" +DEBUG = 1 +async def get_battery_percentage(voltage_mV: float): + max_voltage = 8400.0 + min_voltage = 5000.0 + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage)) * 100 + return max(0, min(100, percentage)) + +async def wait_button_release(): + while hub.buttons.pressed(): + await wait(500) + await wait(1000) + +WALL_DISTANCE = 300 + +async def drive_forward(): + drive_base.drive(1000, 0) + +async def drive_backward(): + drive_base.drive(400, 0) + +async def monitor_distance(): + while True: + distance = await lazer_ranger.distance() + print('Distancing...', distance) + if distance < WALL_DISTANCE: + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + if distance is None: + continue + await wait(50) + +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +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 +- What's on sale + Silo +- Green Key +""" +async def Run1(): + await left_arm.run_angle(2000, -210) + 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_v3() + await solve_silo() + + # return to base + await drive_base.straight(-90) + await drive_base.arc(200, None, -300) + drive_base.stop() + +async def solve_whats_on_sale_v3(): + right_arm.run_angle(500, 30) + await left_arm.run_angle(500, 75, Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(190) + 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) + await left_arm.run_angle(2000, -120) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) + + 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_silo(): + await drive_base.straight(-80) + await drive_base.turn(45) + await drive_base.straight(120) + + SPEED = 10000 + SWING_ANGLE = 60 + + for _ in range(4): + await right_arm.run_angle(SPEED, SWING_ANGLE, Stop.HOLD) + await right_arm.run_angle(SPEED, (-1 * SWING_ANGLE), Stop.HOLD) + + right_arm.run_angle(4000, 60, Stop.HOLD) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + await solve_flag() + + # return to base + await drive_base.turn(55) + await drive_base.straight(-190) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100, 90) + await right_arm.run_target(50, 50) + await wait(800) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(90) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000, -160) + await wait(100) + await drive_base.turn(30) + await right_arm.run_angle(2000, 160) + await drive_base.turn(-30) + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-100) + await drive_base.turn(-5) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70, -90) + await wait(500) + await left_arm.run_angle(100, 120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250, -90) + await left_arm.run_angle(250, 90) + + +""" +Run#2.1 +- Light Blue Key +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + await drive_base.arc(-500, None, 600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50, 50) + await right_arm.run_angle(50, -30) + await drive_base.straight(700) + await drive_base.turn(-40) + await drive_base.straight(-30) + + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_tip_the_scale() + await solve_angler_artifacts() + + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-275, None, 365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000, -800) + await drive_base.straight(-120) + await drive_base.turn(110) + await drive_base.turn(-25) + + +""" +Run #4 +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +- *** USES PID DRIVE *** +""" +async def Run5(): + # Getting the sand down + await drive.pid_straight(550) + await right_arm.run_angle(300, 100) + await drive.pid_straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + await drive.pid_straight(300) + await drive.pid_straight(-200) + await drive.pid_turn(-15) + # Solving statue + await drive.pid_straight(350) + await drive.pid_turn(-104) + await drive.pid_straight(-80) + await left_arm.run_angle(500, -300) + await drive.pid_straight(120) + await drive.pid_turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive.pid_turn(18) + await drive.pid_straight(-100) + await drive.pid_turn(-90) + set_fast_mode() + await drive.pid_straight(900) + set_default_pid_mode() + drive_base.stop() + + +async def solve_salvage_operation(): + await drive_base.straight(500) + await right_arm.run_angle(300, 500) + await drive_base.straight(-75) + await right_arm.run_angle(300, -900) + await drive_base.straight(-350) + await wait(1000) + await drive_base.straight(800) + await drive_base.straight(-200) + await drive_base.turn(-15) + await drive_base.straight(350) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700, 200) + await drive_base.turn(30) + + +""" +Run#6 +- Purple Key +""" +async def Run6_7(): + solve_site_mark_1() + solve_site_mark_2() + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + +async def Run10(): + 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 drive_base.straight(-200) + await left_arm.run_angle(300, -215) + await drive_base.straight(-600) + drive_base.stop() + + +""" +Run#11 +- *** USES PID DRIVE *** +""" +async def Run11(): + await drive.pid_straight(600) + set_precise_mode() + await drive.pid_turn(-30) + await drive.pid_straight(250) + + set_default_pid_mode() + await drive.pid_straight(-100) + await drive.pid_turn(30) + await drive.pid_straight(-600) + drive_base.stop() + + +""" +Run#12 +- *** USES PID DRIVE *** +""" +async def Run12(): + set_fast_mode() + await drive.pid_straight(900) + await drive.pid_turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + set_precise_mode() + await drive.pid_straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive.pid_straight(-100) + set_fast_mode() + await drive.pid_turn(110) + await drive.pid_straight(1000) + set_default_pid_mode() + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: + return "Red" + elif 3 < h < 40 and s > 70: + return "Orange" + elif 47 < h < 56: + return "Yellow" + elif 70 < h < 160: + return "Green" + elif 195 < h < 198: + return "Light_Blue" + elif 210 < h < 225: + return "Blue" + elif 260 < h < 350: + return "Purple" + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if DEBUG: + print(f"button pressed: {pressed}") + + if color == "Green": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + 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})") + + if Button.BLUETOOTH in pressed: + battery_voltage_mV = hub.battery.voltage() + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) + +run_task(main()) \ No newline at end of file diff --git a/competition_codes/AROC/Tisimal.py b/competition_codes/AROC/Tisimal.py new file mode 100644 index 0000000..bb40dd2 --- /dev/null +++ b/competition_codes/AROC/Tisimal.py @@ -0,0 +1,655 @@ +#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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction +right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +""" +Debugging helps +""" +DEBUG = 1 # 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 = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,0) + +async def drive_backward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + drive_base.stop() + print(f"Wall detected at {distance}mm!") + 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) + +# PID Controller Module +class PIDController: + def __init__(self, kp, ki, kd): + self.kp = kp # Proportional gain + self.ki = ki # Integral gain + self.kd = kd # Derivative gain + self.previous_error = 0 + self.integral = 0 + + def calculate(self, desired_value, actual_value, dt): + """ + Calculate PID output + + Args: + desired_value: Target value + actual_value: Current measured value + dt: Time delta in seconds + + Returns: + PID output value + """ + error = desired_value - actual_value + + # Proportional term + proportional = error * self.kp + + # Integral term + self.integral += error * dt + integral = self.integral * self.ki + + # Derivative term + derivative = (error - self.previous_error) / dt * self.kd if dt > 0 else 0 + + # Store current error for next iteration + self.previous_error = error + + # Calculate total output + output = proportional + integral + derivative + + return output + + def reset(self): + """Reset the PID controller""" + self.previous_error = 0 + self.integral = 0 + +# Create PID controllers for Missions 5, 11, and 12 +straight_pid = PIDController(kp=1.2, ki=0.0, kd=0.1) +turn_pid = PIDController(kp=2.0, ki=0.0, kd=0.2) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -210) # Fast movement downward + + # 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_v3() + await solve_silo() + + # return to base + await drive_base.straight(-90) + #await drive_base.turn(-100) + await drive_base.arc(200,None,-300) + drive_base.stop() + +async def solve_whats_on_sale_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(190) + + 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) + + 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_silo(): + await drive_base.straight(-80) + await drive_base.turn(45) + await drive_base.straight(120) + + SPEED = 10000 # speed in degree per second + 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.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, Stop.HOLD) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + await solve_flag() + + # return to base + await drive_base.turn(55) + await drive_base.straight(-190) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100,90) + await right_arm.run_target(50,50) + await wait(800) + # await right_arm.run_angle(50,-30) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(90) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-100) + await drive_base.turn(-5) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250,-90) + await left_arm.run_angle(250,90) + + +""" +Run#2.1 +- Alternate solution for Forge, Who lived here and Heavy Lifting combined +- Light Blue Key +- Different alignment +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50,50) + await right_arm.run_angle(50,-30) + await drive_base.straight(700) + # await drive_base.turn(-30) + # await drive_base.straight(20) + await drive_base.turn(-40) + await drive_base.straight(-30) + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_tip_the_scale() + await solve_angler_artifacts() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-275,None,365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000,-800) + await drive_base.straight(-120) + await drive_base.turn(110) + await drive_base.turn(-25) + + + + + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" +async def Run5(): + # Reset PID controller for this mission + straight_pid.reset() + + # Getting the sand down with PID control + await drive_straight_pid(550) + await right_arm.run_angle(300,100) + await drive_straight_pid(-75) + await right_arm.run_angle(300, -100) + + # Shoving the boat into place + await drive_straight_pid(300) + await drive_straight_pid(-200) + await drive_turn_pid(-15) + + # Solving statue + await drive_straight_pid(350) + await drive_turn_pid(-104) + await drive_straight_pid(-80) + await left_arm.run_angle(500, -300) + await drive_straight_pid(120) + await drive_turn_pid(5) + + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive_turn_pid(18) + await drive_straight_pid(-100) + await drive_turn_pid(-90) + await drive_straight_pid(900) + drive_base.stop() + +async def solve_salvage_operation(): + await drive_base.straight(500) + await right_arm.run_angle(300,500) + await drive_base.straight(-75) + await right_arm.run_angle(300, -900) + await drive_base.straight(-350) + await wait(1000) + await drive_base.straight(800) + await drive_base.straight(-200) + await drive_base.turn(-15) + await drive_base.straight(350) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700,200) + await drive_base.turn(30) + +# PID-enhanced drive functions for Mission 5 +async def drive_straight_pid(distance_mm): + """Drive straight using PID control""" + start_distance = drive_base.distance() + target_distance = start_distance + distance_mm + straight_pid.reset() + + stopwatch = StopWatch() + stopwatch.reset() + + # Drive with PID correction + while abs(drive_base.distance() - target_distance) > 5: # 5mm tolerance + elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds + stopwatch.reset() + + # Calculate PID correction + correction = straight_pid.calculate(target_distance, drive_base.distance(), elapsed_time) + + # Apply correction (limit to reasonable values) + correction = max(min(correction, 100), -100) + + # Drive with correction + drive_base.drive(200, correction) # Base speed 200 mm/s + await wait(10) + + drive_base.stop() + +async def drive_turn_pid(angle_degrees): + """Turn using PID control""" + start_angle = drive_base.angle() + target_angle = start_angle + angle_degrees + turn_pid.reset() + + stopwatch = StopWatch() + stopwatch.reset() + + # Turn with PID correction + while abs(drive_base.angle() - target_angle) > 2: # 2 degree tolerance + elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds + stopwatch.reset() + + # Calculate PID correction + correction = turn_pid.calculate(target_angle, drive_base.angle(), elapsed_time) + + # Apply correction (limit to reasonable values) + correction = max(min(correction, 100), -100) + + # Turn with correction + drive_base.drive(0, correction) # No forward movement, just turning + await wait(10) + + drive_base.stop() + +""" +Run#6 +- Solve 2/3 Site Markings +- Run only if have time +- Purple Key +""" +async def Run6_7(): # experiment with ferris wheel for Site Markings + solve_site_mark_1() + solve_site_mark_2() + #return to base + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + +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 with PID + # Reset PID controllers + straight_pid.reset() + turn_pid.reset() + + # Drive forward with PID + await drive_straight_pid(600) + + # Turn with PID + await drive_turn_pid(-30) + + # Drive forward with PID + await drive_straight_pid(250) + + # Return with PID control + await drive_straight_pid(-100) + await drive_turn_pid(30) + await drive_straight_pid(-600) + drive_base.stop() + +async def Run12(): + # Reset PID controllers for this mission + straight_pid.reset() + turn_pid.reset() + + # Drive forward with PID + await drive_straight_pid(900) + + # Turn with PID + await drive_turn_pid(83) + + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_straight_pid(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_straight_pid(-100) + drive_base.settings(950, 750, 750, 750) + + # Turn with PID + await drive_turn_pid(110) + + # Drive forward with PID + await drive_straight_pid(1000) + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Light_Blue" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + 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": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + 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 + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/RegionalFinal.py b/competition_codes/regional-final/RegionalFinalOld.py similarity index 100% rename from RegionalFinal.py rename to competition_codes/regional-final/RegionalFinalOld.py diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/mukwonago_sectionals_final.py similarity index 89% rename from competition_codes/sectionals/sectional_main_dec_6.py rename to competition_codes/sectionals/mukwonago_sectionals_final.py index c0a04f1..084eb70 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/mukwonago_sectionals_final.py @@ -123,10 +123,10 @@ async def solve_forge(): await drive_base.straight(-47) async def solve_heavy_lifting(): - await right_arm.run_angle(500,-90) # arm down + await right_arm.run_angle(500,-160) # arm down await wait(100) await drive_base.turn(20) # turn right a little bit - await right_arm.run_angle(500,140) #arm up + await right_arm.run_angle(500,160) #arm up await drive_base.turn(-20) #reset position async def solve_who_lived_here(): @@ -176,9 +176,8 @@ async def Run3(): monitor_distance() ) - async def solve_angler_artifacts(): - await drive_base.straight(920) + await drive_base.straight(940) await drive_base.turn(-90,Stop.HOLD) await drive_base.straight(65) #Solve @@ -187,20 +186,25 @@ async def solve_angler_artifacts(): await drive_base.straight(-110) await drive_base.turn(90) - await drive_base.arc(-150,-100, None) + await drive_base.arc(-150,-103, None) async def solve_tip_the_scale(): - await drive_base.straight(135) + await drive_base.turn(3) + await drive_base.straight(142.5) await right_arm.run_angle(800,-150) await right_arm.run_angle(900,150) await drive_base.straight(-100) await drive_base.turn(-65) - await drive_base.straight(320,Stop.COAST_SMART) + await drive_base.straight(300,Stop.COAST_SMART) await drive_base.arc(10,-47, None) #await drive_base.turn(-23, Stop.COAST_SMART) + + + + """ Run #4 - Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal @@ -216,7 +220,11 @@ async def Run4(): await right_arm.run_angle(400, -200) await drive_base.straight(90) await right_arm.run_angle(100, 95) - await drive_base.straight(-875) + #await drive_base.straight(-875) + #return with arc + await drive_base.straight(-600) + await drive_base.arc(10,-47, None) + await drive_base.straight(-400,Stop.COAST_SMART) async def solve_brush_reveal(): await drive_base.straight(700) @@ -244,16 +252,20 @@ async def Run5(): await drive_base.straight(300) await drive_base.straight(-200) await drive_base.turn(-15) + #solving statue await drive_base.straight(350) - await drive_base.turn(-103) + await drive_base.turn(-104) await drive_base.straight(-80) await left_arm.run_angle(500, -900) - await drive_base.straight(105) - await left_arm.run_angle(700, 285) - await drive_base.turn(30) - await drive_base.straight(-60) - await drive_base.turn(80) - await drive_base.straight(-900) + await drive_base.straight(120) + await drive_base.turn(5) + await left_arm.run_angle(500, 290) + await drive_base.turn(18) + await drive_base.straight(-100) + await drive_base.turn(-90) + await drive_base.straight(900) + drive_base.stop() + async def solve_salvage_operation(): await drive_base.straight(500) @@ -283,8 +295,15 @@ Run#6 - Purple Key """ async def Run6_7(): # experiment with ferris wheel for Site Markings - solve_site_mark_1() - solve_site_mark_2() + #solve_site_mark_1() + #solve_site_mark_2() + await drive_base.straight(640) + await drive_base.straight(-150) + await drive_base.turn(-8) + await left_arm.run_angle(500, -520) + await drive_base.straight(300) + await wait(50) + await left_arm.run_angle(500, 700) #return to base await drive_base.straight(-300) drive_base.stop() @@ -352,7 +371,7 @@ async def main(): print('Running Mission 5') await Run5() elif color == "Purple": - print('Running Mission 6') + print('Running Mission 6_7') await Run6_7() elif color == "Light_Blue": print("Running Mission 2_1") diff --git a/competition_codes/state/Updated state.py b/competition_codes/state/Updated state.py new file mode 100644 index 0000000..4b0cebb --- /dev/null +++ b/competition_codes/state/Updated state.py @@ -0,0 +1,513 @@ +#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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction +right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +""" +Debugging helps +""" +DEBUG = 1 # 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) + +async def drive_backward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + drive_base.stop() + print(f"Wall detected at {distance}mm!") + 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 +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # 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) + print(f"Initial left arm angle : {left_arm.angle()}") + + await solve_whats_on_sale_v2() + await solve_silo() + + # return to base + await drive_base.straight(-90) + #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) + + #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) + + 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_silo(): + await drive_base.straight(-80) + await drive_base.turn(42) + await drive_base.straight(95) + + SPEED = 10000 # speed in degree per second + 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.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, Stop.HOLD) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge(): + await right_arm.run_target(50,50) + # await right_arm.run_angle(50,-30) + await drive_base.arc(350,113, None) + + await drive_base.straight(20) + await drive_base.turn(-60) + await drive_base.straight(-47) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(50) + await drive_base.turn(-15) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-50) + await drive_base.turn(-40) + await drive_base.straight(50) + right_arm.run_angle(1000,-160) + await drive_base.turn(-60) + await right_arm.run_angle(2000,160) + +""" +Run#2.1 +- Alternate solution for Forge, Who lived here and Heavy Lifting combined +- Light Blue Key +- Different alignment +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50,50) + await right_arm.run_angle(50,-30) + await drive_base.straight(700) + # await drive_base.turn(-30) + # await drive_base.straight(20) + await drive_base.turn(-40) + await drive_base.straight(-30) + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_angler_artifacts() + await solve_tip_the_scale() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_angler_artifacts(): + await drive_base.straight(870) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(45) + #Solve + drive_base.turn(-10) + await left_arm.run_angle(10000,-750) + await drive_base.straight(-130) + + await drive_base.turn(67) + +async def solve_tip_the_scale(): + await drive_base.straight(-200) + await drive_base.straight(60) + await drive_base.turn(22) + + + + + + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" +async def Run5(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + await drive_base.straight(300) + await drive_base.straight(-200) + await drive_base.turn(-15) + # Solving statue + await drive_base.straight(350) + await drive_base.turn(-104) + await drive_base.straight(-80) + await left_arm.run_angle(500, -300) + await drive_base.straight(120) + await drive_base.turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive_base.turn(18) + await drive_base.straight(-100) + await drive_base.turn(-90) + await drive_base.straight(900) + drive_base.stop() + + +async def solve_salvage_operation(): + await drive_base.straight(500) + await right_arm.run_angle(300,500) + await drive_base.straight(-75) + await right_arm.run_angle(300, -900) + await drive_base.straight(-350) + await wait(1000) + await drive_base.straight(800) + await drive_base.straight(-200) + await drive_base.turn(-15) + await drive_base.straight(350) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700,200) + await drive_base.turn(30) + +""" +Run#6 +- Solve 2/3 Site Markings +- Run only if have time +- Purple Key +""" +async def Run6_7(): # experiment with ferris wheel for Site Markings + solve_site_mark_1() + solve_site_mark_2() + #return to base + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + +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) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-100) + await drive_base.turn(30) + await drive_base.straight(-190) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await left_arm.run_angle(300,-300) + await drive_base.straight(-600) + drive_base.stop() + +async def Run12(): + await drive_base.straight(900) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Light_Blue" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + 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": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + 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 + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/competition_codes/state/run10_experimental.py b/competition_codes/state/run10_experimental.py new file mode 100644 index 0000000..95dd75b --- /dev/null +++ b/competition_codes/state/run10_experimental.py @@ -0,0 +1,16 @@ +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() diff --git a/competition_codes/state/run11_experimental.py b/competition_codes/state/run11_experimental.py new file mode 100644 index 0000000..c342307 --- /dev/null +++ b/competition_codes/state/run11_experimental.py @@ -0,0 +1,16 @@ +await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-180) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await left_arm.run_angle(300,-300) + await drive_base.straight(-600) + drive_base.stop() diff --git a/competition_codes/state/sunprarie_state_main_FINAL.py b/competition_codes/state/sunprarie_state_main_FINAL.py new file mode 100644 index 0000000..04d244c --- /dev/null +++ b/competition_codes/state/sunprarie_state_main_FINAL.py @@ -0,0 +1,513 @@ +#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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction +right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +""" +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) + +async def drive_backward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + drive_base.stop() + print(f"Wall detected at {distance}mm!") + 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 +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -190) # Fast movement downward + + # 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_v3() + await solve_silo() + + # return to base + await drive_base.straight(-90) + #await drive_base.turn(-100) + await drive_base.arc(200,None,-300) + drive_base.stop() + +async def solve_whats_on_sale_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + left_arm.run_target(500,70, 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) + + 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_silo(): + await drive_base.straight(-80) + await drive_base.turn(42) + await drive_base.straight(95) + + SPEED = 10000 # speed in degree per second + 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.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, Stop.HOLD) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge(): + await right_arm.run_target(50,50) + # await right_arm.run_angle(50,-30) + await drive_base.arc(350,113, None) + + await drive_base.straight(20) + await drive_base.turn(-60) + await drive_base.straight(-47) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(50) + await drive_base.turn(-15) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-50) + await drive_base.turn(-40) + await drive_base.straight(50) + right_arm.run_angle(1000,-160) + await drive_base.turn(-60) + await right_arm.run_angle(2000,160) + +""" +Run#2.1 +- Alternate solution for Forge, Who lived here and Heavy Lifting combined +- Light Blue Key +- Different alignment +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50,50) + await right_arm.run_angle(50,-30) + await drive_base.straight(700) + # await drive_base.turn(-30) + # await drive_base.straight(20) + await drive_base.turn(-40) + await drive_base.straight(-30) + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_angler_artifacts() + await solve_tip_the_scale() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_angler_artifacts(): + await drive_base.straight(870) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(45) + #Solve + drive_base.turn(-10) + await left_arm.run_angle(10000,-750) + await drive_base.straight(-130) + + await drive_base.turn(67) + +async def solve_tip_the_scale(): + await drive_base.straight(-200) + await drive_base.straight(60) + await drive_base.turn(22) + + + + + + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" +async def Run5(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + await drive_base.straight(300) + await drive_base.straight(-200) + await drive_base.turn(-15) + # Solving statue + await drive_base.straight(350) + await drive_base.turn(-104) + await drive_base.straight(-80) + await left_arm.run_angle(500, -300) + await drive_base.straight(120) + await drive_base.turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive_base.turn(18) + await drive_base.straight(-100) + await drive_base.turn(-90) + await drive_base.straight(900) + drive_base.stop() + + +async def solve_salvage_operation(): + await drive_base.straight(500) + await right_arm.run_angle(300,500) + await drive_base.straight(-75) + await right_arm.run_angle(300, -900) + await drive_base.straight(-350) + await wait(1000) + await drive_base.straight(800) + await drive_base.straight(-200) + await drive_base.turn(-15) + await drive_base.straight(350) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700,200) + await drive_base.turn(30) + +""" +Run#6 +- Solve 2/3 Site Markings +- Run only if have time +- Purple Key +""" +async def Run6_7(): # experiment with ferris wheel for Site Markings + solve_site_mark_1() + solve_site_mark_2() + #return to base + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + +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) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-230) + await drive_base.turn(30) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + #await drive_base.straight(-80) + #await left_arm.run_angle(300, -75) + await drive_base.arc(-650,None,-800) + drive_base.stop() + +async def Run12(): + await drive_base.straight(900) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Light_Blue" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + 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": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + 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 + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/twist_scrimmage.py b/competition_codes/twist_scrimmage/twist_scrimmage.py similarity index 100% rename from twist_scrimmage.py rename to competition_codes/twist_scrimmage/twist_scrimmage.py diff --git a/config.py b/config.py deleted file mode 100644 index f59fbfb..0000000 --- a/config.py +++ /dev/null @@ -1,40 +0,0 @@ -# config.py - Robot configuration shared across all modules -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor -from pybricks.parameters import Port - -# Initialize hub -hub = PrimeHub() - -# Robot hardware configuration -ROBOT_CONFIG = { - # Drive motors - 'left_motor': Motor(Port.A), - 'right_motor': Motor(Port.B), - - # Attachment motors - 'attachment_motor': Motor(Port.C), - 'lift_motor': Motor(Port.D), - - # Sensors - #'color_left': ColorSensor(Port.E), - 'color_back': ColorSensor(Port.F), - 'ultrasonic': UltrasonicSensor(Port.E), - - # Hub reference - 'hub': hub -} - -# Speed and distance constants -SPEEDS = { - 'FAST': 400, - 'NORMAL': 250, - 'SLOW': 100, - 'PRECISE': 50 -} - -DISTANCES = { - 'TILE_SIZE': 300, # mm per field tile - 'ROBOT_LENGTH': 180, # mm - 'ROBOT_WIDTH': 140 # mm -} \ No newline at end of file diff --git a/final/1main.py b/old_combined/1main.py similarity index 99% rename from final/1main.py rename to old_combined/1main.py index f535603..4912045 100644 --- a/final/1main.py +++ b/old_combined/1main.py @@ -1,4 +1,3 @@ - from pybricks.hubs import PrimeHub from pybricks.pupdevices import Motor, ColorSensor from pybricks.parameters import Port, Stop, Color, Direction diff --git a/final/2main.py b/old_combined/2main.py similarity index 99% rename from final/2main.py rename to old_combined/2main.py index 7c0940d..892e31c 100644 --- a/final/2main.py +++ b/old_combined/2main.py @@ -1,4 +1,3 @@ - from pybricks.hubs import PrimeHub from pybricks.pupdevices import Motor, ColorSensor from pybricks.parameters import Port, Stop, Color, Direction diff --git a/final/3main.py b/old_combined/3main.py similarity index 100% rename from final/3main.py rename to old_combined/3main.py diff --git a/final/4main.py b/old_combined/4main.py similarity index 100% rename from final/4main.py rename to old_combined/4main.py diff --git a/final/main5.py b/old_combined/main5.py similarity index 100% rename from final/main5.py rename to old_combined/main5.py diff --git a/test_10_17_2025.py b/test_10_17_2025.py deleted file mode 100644 index 1d89281..0000000 --- a/test_10_17_2025.py +++ /dev/null @@ -1,303 +0,0 @@ -# Stuff from 10/15/2025 -# Atharv trying with no avail to add more colors to the color sensor logic 😭😭😭😭😭😭😭 -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor -from pybricks.parameters import Port, Stop, Color, Direction -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch, multitask, run_task - -hub = PrimeHub() -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) -left_arm = Motor(Port.C)#, Direction.COUNTERCLOCKWISE) -right_arm = Motor(Port.D) -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180) -drive_base.settings(550,700,100,100) -drive_base.use_gyro(True) -color_sensor = ColorSensor(Port.F) -Color.ORANGE = Color(28, 61, 61) -Color.BLUE = Color(230,100,100) -Color.YELLOW = Color(37, 85, 95) -Color.PURPLE = Color(326, 60, 87) -color_sensor.detectable_colors([Color.ORANGE, Color.BLUE, Color.GREEN, Color.WHITE, Color.RED, Color.YELLOW, Color.NONE, Color.PURPLE]) -hub.speaker.volume(50) # Set the volume of the speaker -color_sensor.detectable_colors() -# Celebration sound types -class CelebrationSound: - VICTORY_FANFARE = 0 - LEVEL_UP = 1 - SUCCESS_CHIME = 2 - TA_DA = 3 - POWER_UP = 4 - RICKROLL_INSPIRED = 5 - -async def play_victory_fanfare(): - """Classic victory fanfare""" - notes = [ - (262, 200), # C4 - (262, 200), # C4 - (262, 200), # C4 - (349, 600), # F4 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(50) -async def play_level_up(): - """Upward scale for level completion""" - notesold = [ - (262, 100), # C4 - (294, 100), # D4 - (330, 100), # E4 - (349, 100), # F4 - (392, 100), # G4 - (440, 100), # A4 - (494, 100), # B4 - (523, 300), # C5 - ] - notes = [ - (277, 100), - (330, 100), - (277, 100), - (554, 100), - (277, 100), - (413, 100), - (330, 100), - (277, 100), - (413, 100), - (277, 100), - (554, 100), - (413, 100), - (277, 100), - (413, 100), - (554, 100), - (413, 100) - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - #await wait(20) -async def play_success_chime(): - """Simple success notification""" - notes = [ - (523, 150), # C5 - (659, 150), # E5 - (784, 300), # G5 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(50) -async def play_ta_da(): - """Classic "ta-da!" sound""" - notes = [ - (392, 200), # G4 - (523, 400), # C5 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(100) -async def play_power_up(): - """Rising power-up sound""" - for freq in range(200, 800, 50): - await hub.speaker.beep(freq, 50) - await wait(10) - await hub.speaker.beep(1000, 200) -async def play_rickroll_inspired(): - """Fun 80s-style dance beat inspired sound""" - # Upbeat bouncy rhythm - pattern = [ - (392, 200), (440, 200), (494, 200), (523, 200), - (440, 200), (392, 200), (349, 200), (392, 300), - (440, 200), (392, 200), (349, 200), (330, 400), - ] - - for freq, duration in pattern: - await hub.speaker.beep(freq, duration) - await wait(50) -async def celebrate_mission_complete(sound_type=CelebrationSound.SUCCESS_CHIME): - """ - Main celebration function to call after completing a mission. - Plays a sound and shows light animation. - - Args: - sound_type: CelebrationSound enum value (default: SUCCESS_CHIME) - """ - # Light show - hub.light.on(Color.GREEN) - - # Play the selected celebration sound - if sound_type == CelebrationSound.VICTORY_FANFARE: - await play_victory_fanfare() - elif sound_type == CelebrationSound.LEVEL_UP: - await play_level_up() - elif sound_type == CelebrationSound.SUCCESS_CHIME: - await play_success_chime() - elif sound_type == CelebrationSound.TA_DA: - await play_ta_da() - elif sound_type == CelebrationSound.POWER_UP: - await play_power_up() - elif sound_type == CelebrationSound.RICKROLL_INSPIRED: - await play_rickroll_inspired() - else: - await play_success_chime() # Default fallback - - # Blink the light - for _ in range(3): - hub.light.off() - await wait(100) - hub.light.on(Color.GREEN) - await wait(100) - - hub.light.off() - -async def Run1(): - left_arm.run_angle(1000, -300) - right_arm.run_angle(1000,500) - await drive_base.straight(320) - await right_arm.run_angle(5000,-500, Stop.HOLD) - await right_arm.run_angle(5000,500, Stop.HOLD) - await right_arm.run_angle(5000,-500, Stop.HOLD) - await right_arm.run_angle(5000,500, Stop.HOLD) - await right_arm.run_angle(5000,-500, Stop.HOLD) - await drive_base.turn(-20) - await drive_base.straight(277) - await drive_base.turn(20) - await drive_base.straight(65) - await drive_base.turn(-30) - right_arm.run_angle(50,500) - await drive_base.turn(45) - await drive_base.straight(-145) - await drive_base.turn(-60) - await drive_base.straight(90) - await left_arm.run_angle(1000, 450) - await drive_base.straight(-145) - await left_arm.run_angle(1000,-450) - await drive_base.straight(10) - await drive_base.turn(35) - await drive_base.straight(-700) - -async def Run2(): - await drive_base.straight(200) - await drive_base.turn(-20) - await drive_base.straight(525) - await drive_base.turn(60) - - 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) - await drive_base.straight(-100) - await drive_base.turn(-100) - await drive_base.straight(-750) - - -async def Run3(): - await drive_base.straight(920) - await drive_base.turn(-90) - await drive_base.straight(60) - drive_base.turn(-10) - await left_arm.run_angle(10000,-4000) - await drive_base.straight(-110) - await drive_base.turn(90) - await drive_base.straight(2000) - -async def Run4(): - await drive_base.straight(519) - await drive_base.straight(519) - await arm_motor_left.run_angle(-10000, 300) - await arm_motor_left.run_angle(10000, 600) - await drive_base.straight(160) - await drive_base.turn(-30) - await drive_base.straight(50) - await arm_motor.run_angle(3000, 3000) - await drive_base.straight(-150) - await drive_base.turn(135) - await drive_base.straight(50) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(-100) - await drive_base.turn(-54) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(250) - await drive_base.turn(-5) - await arm_motor.run_angle(10000, 7000) - await drive_base.straight(-50) - await drive_base.turn(68) - await arm_motor.run_angle(10000, -6000) - await drive_base.straight(200) - await arm_motor.run_angle(10000, 4000) - await drive_base.turn(-40) - await arm_motor.run_angle(10000, 7000) - -async def Run5(): - 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 Run6(): - left_arm.run_angle(500,200) - right_arm.run_angle(500,200) - await drive_base.straight(70) - - await drive_base.turn(-55) - await drive_base.straight(900) - await drive_base.turn(92.5) - - await drive_base.straight(75) - await drive_base.straight(21) - await right_arm.run_angle(500,-250) - await right_arm.run_angle(500,250) - await drive_base.turn(55) - - await left_arm.run_angle(300,-400) - - await drive_base.turn(46.5) - await drive_base.turn(-40) - await drive_base.straight(900) -async def main(): - while True: - - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - color_reflected_percent = await color_sensor.reflection() - print(color_reflected_percent) - - color_detected = await color_sensor.color() - print(f'Detected color: {color_detected.h}, {color_detected.s}, {color_detected.v}') - if color_reflected_percent > 1: - if color_detected == Color.GREEN: - print('Running Mission 1') - await Run1() - #await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE) - elif color_detected == Color.WHITE: - print('Running Mission 2') - await Run2() - #await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED) - elif color_detected == Color.YELLOW: - print('Running Mission 3') - await Run3() - #await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME ) - elif color_detected == Color.BLUE: - print('Running Mission 4') - await Run4() - #await celebrate_mission_complete(CelebrationSound.POWER_UP) - elif color_detected == Color.ORANGE: - print('Running Mission 5') - await Run5() - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - elif color_detected == Color.PURPLE: - print('Running Mission 6 (this is ayaan\'s code)') - await Run6() - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - else: - hub.light.off() - left_motor.stop() - right_motor.stop() - await wait(1000) #prevent loop from iterating fast -# Main execution loop -run_task(main()) diff --git a/utils/color_sensor_navi.py b/utils/color_sensor_navi.py deleted file mode 100644 index 54bb542..0000000 --- a/utils/color_sensor_navi.py +++ /dev/null @@ -1,41 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch - -hub = PrimeHub() - -# Robot hardware configuration
ROBOT_CONFIG = {
 # Drive motors
 'left_motor': Motor(Port.A),
 'right_motor': Motor(Port.B),
 
 # Attachment motors
 'attachment_motor': Motor(Port.C),
 'lift_motor': Motor(Port.D),
 
 # Sensors
 'color_left': ColorSensor(Port.E),
 'color_right': ColorSensor(Port.F),
 'ultrasonic': UltrasonicSensor(Port.S1),
 
 # Hub reference
 'hub': hub
}

# Speed and distance constants
SPEEDS = {
 'FAST': 400,
 'NORMAL': 250,
 'SLOW': 100,
 'PRECISE': 50
}

DISTANCES = {
 'TILE_SIZE': 300, # mm per field tile
 'ROBOT_LENGTH': 180, # mm
 'ROBOT_WIDTH': 140 # mm
} - - -def mission_run_1(): - print('Running missions in Run 1') - -def mission_run_2(): - print('Running missions in Run 2') - -def mission_run_3(): - print('Running missions in Run 3') - -# In main.py - sensor-based navigation -def main(self): - """Use color sensor to select runs""" - print("Place colored object in front of sensor:") - print("RED=Run1, GREEN=Run2, BLUE=Run3, YELLOW=Test") - while True: - color = ROBOT_CONFIG['color_left'].color() - if color == Color.RED: - mission_run_1() - break - elif color == Color.GREEN: - mission_run_2() - break - elif color == Color.BLUE: - mission_run_3() - break - elif color == Color.YELLOW: - self.test_mode() - break - wait(1000) -main() diff --git a/utils/constants.py b/utils/constants.py deleted file mode 100644 index f74d325..0000000 --- a/utils/constants.py +++ /dev/null @@ -1,13 +0,0 @@ -#Speed and distance constants -SPEEDS = { - 'FAST': 400, - 'NORMAL': 250, - 'SLOW': 100, - 'PRECISE': 50 -} - -DISTANCES = { - 'TILE_SIZE': 300, # mm per field tile - 'ROBOT_LENGTH': 180, # mm - 'ROBOT_WIDTH': 140 # mm -} \ No newline at end of file diff --git a/utils/starter_drivebase_code.py b/utils/starter_drivebase_code.py deleted file mode 100644 index d7cb0b7..0000000 --- a/utils/starter_drivebase_code.py +++ /dev/null @@ -1,36 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch - -hub = PrimeHub() - -# Initialize both motors. In this example, the motor on the -# left must turn counterclockwise to make the robot go forward. -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) - -arm_motor = Motor(Port.E, Direction.CLOCKWISE) -arm_motor.run_angle(299,90, Stop.HOLD) -# Initialize the drive base. In this example, the wheel diameter is 56mm. -# The distance between the two wheel-ground contact points is 112mm. -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=112) - -print('The default settings are: ' + str(drive_base.settings())) -drive_base.settings(100,1000,166,750) -# Optionally, uncomment the line below to use the gyro for improved accuracy. -drive_base.use_gyro(True) - -# Drive forward by 500mm (half a meter). -drive_base.straight(500) - -# Turn around clockwise by 180 degrees. -drive_base.turn(180) - -# Drive forward again to get back to the start. -drive_base.straight(500) - -# Turn around counterclockwise. -drive_base.turn(-180) -arm_motor.run_angle(299,-90, Stop.HOLD) \ No newline at end of file