bioglow_solutions/discovery-phase/missions/Showcase_codes.py

"""
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(300)
    await right_arm.run_angle(300,120)
    await drive_base.straight(290)
    await drive_base.turn(50)
    await drive_base.straight(250)
    await drive_base.turn(40)
    set_speed(100,200,300,200)
    await drive_base.straight(170)
    await drive_base.straight(-135)
    await drive_base.turn(80)
    await drive_base.straight(230)
    set_speed(600, 500, 300, 200)
    await drive_base.straight(-145)
    await multitask(
        drive_base.turn(-36),
        right_arm.run_angle(200,-140)
    )
    set_speed(100,200,300,200)
    await drive_base.straight(230)
    await right_arm.run_angle(300,160)
    await wait(200)
    await drive_base.turn(-90)
    await drive_base.straight(-170)
    set_speed(600,500,300,200)
    await drive_base.straight(100)
    await drive_base.arc(800,None,-900)


async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh

    #Run
    await drive_base.straight(550)#Works (600(Previous))
    await drive_base.straight(-200)
    await drive_base.turn(45)
    await drive_base.straight(575)
    await drive_base.turn(-90)#-90 for Coach Cisco table
    await left_arm.run_angle(200,270)
    await drive_base.straight(300)
    await left_arm.run_angle(200,-265)
    await right_arm.run_angle(200, -200)
    await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
    await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
    await right_arm.run_angle(1500, 180)
    await drive_base.turn(30)
    await drive_base.straight(-1000)
    

async def Run5():
    

async def Run6_7():  
    

async def Run10():
    

async def Run11():  # Interchangable Mission - Parthiv

    await left_arm.run_angle(200,-80)
    await drive_base.arc(-350,angle=90)
    await drive_base.turn(50)
    await drive_base.straight(238)
    await drive_base.turn(-33)
    await drive_base.straight(85)
    await drive_base.straight(-33)
    await multitask(
        left_arm.run_angle(200,80),
        right_arm.run_angle(200,-100)
    )
    await drive_base.straight(-60)
    await right_arm.run_angle(200,100)
    await drive_base.turn(30)
    await drive_base.straight(-775)
    await drive_base.stop

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())