414 lines
14 KiB
Python
414 lines
14 KiB
Python
"""
|
|
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(): #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():
|
|
|
|
|
|
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()) |