diff --git a/discovery-phase/missions/Showcase_codes.py b/discovery-phase/missions/Showcase_codes.py new file mode 100644 index 0000000..6de9cda --- /dev/null +++ b/discovery-phase/missions/Showcase_codes.py @@ -0,0 +1,398 @@ +""" +PID Straight Drive — heading-corrected forward / backward movement. + +Improvements over baseline: + • Derivative on measurement (not error) — no derivative kick on setpoint change + • Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the + derivative term (dividing by 0.02 was silently multiplying KD by 50) + • prev_error seeded from first real heading — eliminates first-tick spike + • Per-motor speed clamping — prevents one side pivoting at low speeds + • Deceleration ramp — smooth stop in the last RAMP_MM millimetres + • Dual-motor exit + timeout — survives stall on either wheel + • Integral freeze near target — stops windup right where it matters most +""" + +from pybricks.tools import StopWatch, wait +import umath + +WHEEL_DIAMETER = 68.8 # mm — adjust to your wheel (Done) +AXLE_TRACK = 180 # mm — used only if you add turning later (Done) +LOOP_MS = 20 # fixed control loop period +RAMP_MM = 50 # begin decelerating this far from target +MIN_SPEED = 80 # floor speed during ramp (avoid stall) +TIMEOUT_MS = 8_000 # safety abort +# Tuning sequence + +KP = 4.8 # Wide chassis = stable platform, can afford higher P gain + # without oscillation. Pushes harder through corrections. + +KI = 0.012 # Slightly lower than default — large wheels cover distance + # fast so the integrator has less time to build up per run. + # Raise to 0.012 if you see consistent sideways drift at end. + +KD = 7.5 # Higher than default. Big wheels = more rotational momentum, + # needs stronger damping to prevent overshoot on corrections. + +def _mm_to_deg(mm: float) -> float: + return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0 + + +def _motor_avg_angle(m1, m2) -> float: + return (abs(m1.angle()) + abs(m2.angle())) / 2.0 + + +async def pid_straight(distance_mm: float, speed: int = 600): + """ + Drive straight for distance_mm at speed (deg/s). + Positive = forward, negative = reverse. + """ + direction = 1 if distance_mm >= 0 else -1 + degrees_need = _mm_to_deg(distance_mm) + target_hdg = hub.imu.heading() + + left_motor.reset_angle(0) + right_motor.reset_angle(0) + + # ── seed integral and prev_error from the real opening heading ── + first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start, + integral = 0.0 # non-zero if robot is tilted + prev_heading = hub.imu.heading() # for derivative-on-measurement + + watchdog = StopWatch() + + while True: + traveled = _motor_avg_angle(left_motor, right_motor) + + # ── exit: distance reached or timeout ── + if traveled >= degrees_need: + break + if watchdog.time() > TIMEOUT_MS: + break + + # ── heading error with wrap ── + heading = hub.imu.heading() + error = target_hdg - heading + if error > 180: error -= 360 + if error < -180: error += 360 + + # ── derivative on measurement — no kick when target changes ── + d_heading = heading - prev_heading # raw delta (deg / loop) + if d_heading > 180: d_heading -= 360 + if d_heading < -180: d_heading += 360 + prev_heading = heading + + # ── integral with freeze near target ── + near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2) + if not near_target: + integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt + + # ── PID output ── + # derivative term: -KD * d_heading (measurement, not error difference) + correction = KP * error + KI * integral - KD * d_heading + correction = max(-200.0, min(200.0, correction)) + + # ── ramp speed near end ── + remaining = degrees_need - traveled + ramp_degrees = _mm_to_deg(RAMP_MM) + if remaining < ramp_degrees and ramp_degrees > 0: + ramp_factor = max(float(MIN_SPEED) / speed, + remaining / ramp_degrees) + run_speed = int(speed * ramp_factor) + else: + run_speed = speed + + # ── per-motor clamping — prevents pivot at low speeds ── + l_cmd = max(-speed, min(speed, direction * run_speed + int(correction))) + r_cmd = max(-speed, min(speed, direction * run_speed - int(correction))) + + left_motor.run(l_cmd) + right_motor.run(r_cmd) + await wait(LOOP_MS) + + left_motor.brake() + right_motor.brake() + +#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 = 62.4 # mm (adjust for your wheels) +AXLE_TRACK = 150 # 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(stop_mm: int = WALL_DISTANCE): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...', distance) + + if distance is None: + await wait(50) + continue + if distance < stop_mm: + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + + # 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) + +async def multilift_up(rotation_angle): + await multitask( + right_arm.run_angle(400,rotation_angle), + left_arm.run_angle(400,-1*rotation_angle) + ) + +async def multilift_down(rotation_angle): + await multitask( + right_arm.run_angle(1000,-1*rotation_angle), + left_arm.run_angle(1000,rotation_angle) + ) + + +async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes + + await set_speed(600,200,300,200) + await drive_base.straight(500) + await multitask( + drive_base.straight(-200), + right_arm.run_angle(400,-150) + ) + await set_speed(600,500,100,200) + await drive_base.turn(45) + await drive_base.straight(346) + await drive_base.turn(-68) + await multitask( + left_arm.run_angle(600,240), + set_speed(300,200,300,200), + drive_base.straight(280) + ) + await left_arm.run_angle(600,-100) + await drive_base.turn(22) + await drive_base.straight(-100) + await right_arm.run_angle(5000,45) + await set_speed(600,500,100,200) + await drive_base.straight(200) + await drive_base.arc(-1500,None,-1000) + +async def Run2(): # Exploding Seeds - Johannes + + await multitask( + right_arm.run_angle(400,-100) + left_arm.run_angle(300,100) + drive_base.arc(150,None,115) + ) + await drive_base.straight(270) + await left_arm.run_angle(300,-100) + await drive_base.straight(-100) + await left_arm.run_angle(300,-100) + await drive_base.arc(-1000,None,-800) + + +async def Run2_1(): # Drone Survey, Experimental - Johannes + + await multitask( + right_arm.run_angle(400,100), + set_speed(600,200,300,200), + drive_base.straight(300) + ) + set_speed(600,500,300,200) + await drive_base.straight(470) + await drive_base.turn(-15) + await drive_base.straight(55) + await drive_base.straight(-280) + await drive_base.turn(15) + await right_arm.run_angle(400,-100) + await drive_base.straight(40) + await drive_base.turn(-15) + await drive_base.straight(50) + await drive_base.turn(-25) + + +async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes + + await drive_base.straight(110) + await drive_base.turn(-45) + await drive_base.straight(240) + await right_arm.run_angle(300,120) + await drive_base.straight(290) + await drive_base.turn(50) + await drive_base.straight(300) + await drive_base.turn(40) + set_speed(100,200,300,200) + await drive_base.straight(130) + await drive_base.straight(-115) + await drive_base.turn(87) + await drive_base.straight(230) + set_speed(600, 500, 300, 200) + await drive_base.straight(-135) + await multitask( + drive_base.turn(-40), + right_arm.run_angle(100,-110) + ) + set_speed(100,200,300,200) + await drive_base.straight(220) + await right_arm.run_angle(300,85) + await wait(500) + await drive_base.straight(142) + await drive_base.arc(110,None,-166) + await drive_base.straight(140) + await drive_base.straight(-150) + set_speed(600,500,300,200) + await drive_base.straight(150) + await drive_base.arc(800,None,-900) + + +async def Run4(): + + +async def Run5(): + + +async def Run6_7(): + + +async def Run10(): + + +async def Run11(): + + +async def Run12(): # Experimental Research Platform Lifter + + await multilift_up(1200) + await multilift_down(300) + await drive_base.straight(50) + + +# 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 Run3() + elif color == "Purple": + print('Running Mission 11') + await Run11() + elif color == "Light_Blue": + print("Running Mission 12") + 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