Update competition_codes/AROC/Nationals_Main.py
This commit is contained in:
@@ -1,3 +1,117 @@
|
||||
"""
|
||||
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
|
||||
@@ -20,7 +134,7 @@ color_sensor = ColorSensor(Port.F)
|
||||
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.settings(600, 500, 300, 200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
"""
|
||||
@@ -324,36 +438,26 @@ async def solve_mineshaft_explorer():
|
||||
await drive_base.straight(84)
|
||||
await right_arm.run_angle(300, 90)
|
||||
|
||||
"""
|
||||
Run#5
|
||||
- Solves Salvage Operation + Statue Rebuild
|
||||
- Orange Key
|
||||
"""
|
||||
# Solves Salvage Operation + Statue Rebuild andOrange Key
|
||||
async def Run5():
|
||||
# Getting the sand down
|
||||
await drive_base.straight(550)
|
||||
# Getting the sand down with PID control for precision
|
||||
await pid_straight(550, 400) # Forward 550mm
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_base.straight(-75)
|
||||
await pid_straight(-75, 300) # Backward 75mm
|
||||
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)
|
||||
await pid_straight(300, 400) # Forward 300mm
|
||||
await pid_straight(-200, 300) # Backward 200mm
|
||||
await drive_base.turn(-15) # Keep regular turn for small angles
|
||||
|
||||
# Solving statue
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-104)
|
||||
await drive_base.straight(-80)
|
||||
await pid_straight(350, 400) # Forward 350mm
|
||||
await drive_base.turn(-104) # Regular turn
|
||||
await pid_straight(-80, 300) # Backward 80mm
|
||||
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()
|
||||
|
||||
await pid_straight(120, 300) # Forward 120mm
|
||||
await drive_base.turn(5)
|
||||
|
||||
async def solve_salvage_operation():
|
||||
await drive_base.straight(500)
|
||||
@@ -420,38 +524,30 @@ async def Run10(): # experimental map reveal attachment
|
||||
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)
|
||||
async def Run11(): # experimental surface brushing attachment with PID straight
|
||||
# Using PID straight drive for better precision on long runs
|
||||
await pid_straight(665, 600) # Forward 600mm with PID
|
||||
await drive_base.turn(-35) # Regular turn for small angles
|
||||
await pid_straight(200, 200) # Forward 250mm with PID
|
||||
await pid_straight(-200, 300) # Backward 100mm with PID
|
||||
await drive_base.turn(35) # Regular turn
|
||||
await pid_straight(-665, 500) # Fast backward 600mm with PID
|
||||
drive_base.stop()
|
||||
|
||||
async def Run12():
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(83)
|
||||
# Using PID straight drive for precision on long runs
|
||||
await pid_straight(900) # Forward 900mm with PID
|
||||
await drive_base.turn(89.9) # Regular turn
|
||||
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)
|
||||
await pid_straight(120, 200) # Short precise forward with PID
|
||||
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)
|
||||
|
||||
await pid_straight(-120, 200) # Short precise backward with PID
|
||||
await drive_base.turn(90) # Regular turn
|
||||
await pid_straight(900) # Long return to base with PID
|
||||
drive_base.stop()
|
||||
|
||||
# Function to classify color based on HSV
|
||||
def detect_color(h, s, v, reflected):
|
||||
@@ -505,10 +601,10 @@ async def main():
|
||||
print('Running Mission 5')
|
||||
await Run5()
|
||||
elif color == "Purple":
|
||||
print('Running Mission 6')
|
||||
print('Running Mission 11')
|
||||
await Run11()
|
||||
elif color == "Light_Blue":
|
||||
print("Running Mission 2_1")
|
||||
print("Running Mission 12")
|
||||
await Run12()
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
|
||||
Reference in New Issue
Block a user