The-South_Awakens
Has an advanced PID module, already implemented in RED Side.
This commit is contained in:
745
competition_codes/AROC/The_South-Awakens.py
Normal file
745
competition_codes/AROC/The_South-Awakens.py
Normal file
@@ -0,0 +1,745 @@
|
|||||||
|
#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)
|
||||||
|
|
||||||
|
# ─────────────────────────────────────────────────────────────
|
||||||
|
# Advanced PID Turn Controller
|
||||||
|
# ─────────────────────────────────────────────────────────────
|
||||||
|
# Tuning guide:
|
||||||
|
# Kp – start at 3.0, increase if turn is sluggish
|
||||||
|
# Ki – keep small (0.05–0.15); fights steady-state drift only
|
||||||
|
# Kd – start at 0.8; increase to kill overshoot
|
||||||
|
# DEADBAND – degrees of "close enough" before hard brake (2–4°)
|
||||||
|
# MAX_TURN_SPEED – degrees/sec cap; 200–400 for normal turns
|
||||||
|
# INTEGRAL_CLAMP – prevents windup from accumulated small errors
|
||||||
|
|
||||||
|
PID_Kp = 3.0
|
||||||
|
PID_Ki = 0.08
|
||||||
|
PID_Kd = 1.2
|
||||||
|
DEADBAND = 2.5 # degrees
|
||||||
|
MAX_TURN_SPEED = 350 # deg/s
|
||||||
|
INTEGRAL_CLAMP = 25 # max integral contribution in deg/s
|
||||||
|
SETTLE_MS = 60 # ms to confirm hold inside deadband
|
||||||
|
|
||||||
|
def _clamp(val: float, lo: float, hi: float) -> float:
|
||||||
|
return max(lo, min(hi, val))
|
||||||
|
|
||||||
|
def _shortest_angle(error: float) -> float:
|
||||||
|
"""Wrap error to [-180, 180] so turns never go the long way."""
|
||||||
|
while error > 180:
|
||||||
|
error -= 360
|
||||||
|
while error < -180:
|
||||||
|
error += 360
|
||||||
|
return error
|
||||||
|
|
||||||
|
async def gyro_turn(target_degrees: float, timeout_ms: int = 3000):
|
||||||
|
"""
|
||||||
|
Turn the robot to an absolute heading using a PID loop
|
||||||
|
driven by the hub IMU.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
target_degrees: absolute heading to reach (degrees)
|
||||||
|
timeout_ms: failsafe — stops if turn takes too long
|
||||||
|
"""
|
||||||
|
hub.imu.reset_heading(0)
|
||||||
|
current_heading = 0.0
|
||||||
|
|
||||||
|
integral = 0.0
|
||||||
|
prev_error = 0.0
|
||||||
|
settled_ms = 0
|
||||||
|
|
||||||
|
clock = StopWatch()
|
||||||
|
dt_watch = StopWatch()
|
||||||
|
clock.reset()
|
||||||
|
dt_watch.reset()
|
||||||
|
|
||||||
|
while clock.time() < timeout_ms:
|
||||||
|
# ── 1. Read heading ─────────────────────────────────
|
||||||
|
current_heading = hub.imu.heading()
|
||||||
|
error = _shortest_angle(target_degrees - current_heading)
|
||||||
|
|
||||||
|
# ── 2. Deadband check ───────────────────────────────
|
||||||
|
if abs(error) < DEADBAND:
|
||||||
|
settled_ms += 10
|
||||||
|
if settled_ms >= SETTLE_MS:
|
||||||
|
break # confirmed stable inside target
|
||||||
|
drive_base.drive(0, 0)
|
||||||
|
await wait(10)
|
||||||
|
continue
|
||||||
|
else:
|
||||||
|
settled_ms = 0 # reset settle counter on drift
|
||||||
|
|
||||||
|
# ── 3. dt in seconds (for consistent Ki/Kd scaling) ─
|
||||||
|
dt = dt_watch.time() / 1000.0
|
||||||
|
dt_watch.reset()
|
||||||
|
if dt <= 0 or dt > 0.5: # skip bad first/stall tick
|
||||||
|
dt = 0.02
|
||||||
|
|
||||||
|
# ── 4. P term ───────────────────────────────────────
|
||||||
|
P = PID_Kp * error
|
||||||
|
|
||||||
|
# ── 5. I term with anti-windup clamp ────────────────
|
||||||
|
integral += error * dt
|
||||||
|
integral = _clamp(integral, -INTEGRAL_CLAMP / PID_Ki,
|
||||||
|
INTEGRAL_CLAMP / PID_Ki)
|
||||||
|
I = PID_Ki * integral
|
||||||
|
|
||||||
|
# ── 6. D term (derivative-on-measurement, not error) ─
|
||||||
|
# Using (error - prev_error)/dt avoids "derivative kick"
|
||||||
|
# when the target suddenly changes.
|
||||||
|
D = PID_Kd * (error - prev_error) / dt
|
||||||
|
prev_error = error
|
||||||
|
|
||||||
|
# ── 7. Sum and clamp total output ───────────────────
|
||||||
|
output = _clamp(P + I + D,
|
||||||
|
-MAX_TURN_SPEED,
|
||||||
|
MAX_TURN_SPEED)
|
||||||
|
|
||||||
|
# ── 8. Drive with turn-only command ─────────────────
|
||||||
|
drive_base.drive(0, output)
|
||||||
|
|
||||||
|
await wait(10) # 100 Hz loop
|
||||||
|
|
||||||
|
# Hard brake — holds position with motor hold
|
||||||
|
drive_base.stop()
|
||||||
|
await wait(50) # brief hold for gyro to settle
|
||||||
|
|
||||||
|
# ─────────────────────────────────────────────────────────────
|
||||||
|
# Advanced PID Straight Controller
|
||||||
|
# ─────────────────────────────────────────────────────────────
|
||||||
|
# Tuning guide:
|
||||||
|
# STRAIGHT_Kp – main speed push, start at 2.0
|
||||||
|
# STRAIGHT_Ki – fights distance undershoot, keep tiny
|
||||||
|
# STRAIGHT_Kd – smooths deceleration, start at 0.5
|
||||||
|
# HEADING_Kp – how hard it steers to stay straight
|
||||||
|
# STRAIGHT_DEADBAND – mm of "close enough" before braking
|
||||||
|
|
||||||
|
STRAIGHT_Kp = 2.0
|
||||||
|
STRAIGHT_Ki = 0.02
|
||||||
|
STRAIGHT_Kd = 0.5
|
||||||
|
HEADING_Kp = 3.5 # steering correction gain
|
||||||
|
STRAIGHT_DEADBAND = 5 # mm
|
||||||
|
STRAIGHT_CLAMP = 20 # anti-windup cap
|
||||||
|
MAX_STRAIGHT_SPEED = 800 # mm/s cap
|
||||||
|
|
||||||
|
async def gyro_straight(distance_mm: float, speed: int = 600, timeout_ms: int = 5000):
|
||||||
|
"""
|
||||||
|
Drive straight a precise distance using dual PID:
|
||||||
|
- Distance PID → controls forward speed
|
||||||
|
- Heading PID → corrects steering drift in real time
|
||||||
|
|
||||||
|
Args:
|
||||||
|
distance_mm: how far to travel (negative = backward)
|
||||||
|
speed: max forward speed in mm/s
|
||||||
|
timeout_ms: failsafe cutoff
|
||||||
|
"""
|
||||||
|
# ── Reset everything ────────────────────────────────────
|
||||||
|
hub.imu.reset_heading(0)
|
||||||
|
left_motor.reset_angle(0)
|
||||||
|
right_motor.reset_angle(0)
|
||||||
|
|
||||||
|
target_heading = 0.0
|
||||||
|
integral = 0.0
|
||||||
|
prev_dist_error = 0.0
|
||||||
|
settled_ms = 0
|
||||||
|
|
||||||
|
clock = StopWatch()
|
||||||
|
dt_watch = StopWatch()
|
||||||
|
clock.reset()
|
||||||
|
dt_watch.reset()
|
||||||
|
|
||||||
|
# Convert distance to motor degrees
|
||||||
|
wheel_circumference = umath.pi * WHEEL_DIAMETER # mm per full rotation
|
||||||
|
target_deg = (distance_mm / wheel_circumference) * 360.0
|
||||||
|
|
||||||
|
while clock.time() < timeout_ms:
|
||||||
|
|
||||||
|
# ── 1. Current position from motor encoders ──────────
|
||||||
|
avg_angle = (left_motor.angle() + right_motor.angle()) / 2.0
|
||||||
|
dist_error = target_deg - avg_angle
|
||||||
|
|
||||||
|
# ── 2. Deadband check ────────────────────────────────
|
||||||
|
dist_error_mm = dist_error / 360.0 * wheel_circumference
|
||||||
|
if abs(dist_error_mm) < STRAIGHT_DEADBAND:
|
||||||
|
settled_ms += 10
|
||||||
|
if settled_ms >= 60:
|
||||||
|
break
|
||||||
|
left_motor.hold()
|
||||||
|
right_motor.hold()
|
||||||
|
await wait(10)
|
||||||
|
continue
|
||||||
|
else:
|
||||||
|
settled_ms = 0
|
||||||
|
|
||||||
|
# ── 3. dt ────────────────────────────────────────────
|
||||||
|
dt = dt_watch.time() / 1000.0
|
||||||
|
dt_watch.reset()
|
||||||
|
if dt <= 0 or dt > 0.5:
|
||||||
|
dt = 0.02
|
||||||
|
|
||||||
|
# ── 4. Distance PID ──────────────────────────────────
|
||||||
|
P_dist = STRAIGHT_Kp * dist_error_mm
|
||||||
|
|
||||||
|
integral += dist_error_mm * dt
|
||||||
|
integral = _clamp(integral,
|
||||||
|
-STRAIGHT_CLAMP / STRAIGHT_Ki,
|
||||||
|
STRAIGHT_CLAMP / STRAIGHT_Ki)
|
||||||
|
I_dist = STRAIGHT_Ki * integral
|
||||||
|
|
||||||
|
D_dist = STRAIGHT_Kd * (dist_error_mm - prev_dist_error) / dt
|
||||||
|
prev_dist_error = dist_error_mm
|
||||||
|
|
||||||
|
forward_speed = _clamp(P_dist + I_dist + D_dist,
|
||||||
|
-speed, speed)
|
||||||
|
|
||||||
|
# ── 5. Heading PID (steering correction) ─────────────
|
||||||
|
heading_error = target_heading - hub.imu.heading()
|
||||||
|
heading_error = _shortest_angle(heading_error)
|
||||||
|
steering = HEADING_Kp * heading_error
|
||||||
|
|
||||||
|
# ── 6. Mix forward + steering into each motor ────────
|
||||||
|
# Positive steering correction speeds up left, slows right
|
||||||
|
left_speed = _clamp(forward_speed + steering,
|
||||||
|
-MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED)
|
||||||
|
right_speed = _clamp(forward_speed - steering,
|
||||||
|
-MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED)
|
||||||
|
|
||||||
|
left_motor.run(left_speed)
|
||||||
|
right_motor.run(right_speed)
|
||||||
|
|
||||||
|
await wait(10) # 100 Hz loop
|
||||||
|
|
||||||
|
# ── Hard brake ───────────────────────────────────────────
|
||||||
|
left_motor.hold()
|
||||||
|
right_motor.hold()
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
"""
|
||||||
|
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():
|
||||||
|
# Getting the sand down
|
||||||
|
await gyro_straight(550)
|
||||||
|
await right_arm.run_angle(300,100)
|
||||||
|
await gyro_straight(-75)
|
||||||
|
await right_arm.run_angle(300, -100)
|
||||||
|
# Shoving the boat into place
|
||||||
|
await gyro_straight(300)
|
||||||
|
await gyro_straight(-200)
|
||||||
|
await gyro_turn(-15)
|
||||||
|
# Solving statue
|
||||||
|
await gyro_straight(350)
|
||||||
|
await gyro_turn(-104)
|
||||||
|
await gyro_straight(-80)
|
||||||
|
await left_arm.run_angle(500, -300)
|
||||||
|
await gyro_straight(120)
|
||||||
|
await gyro_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 gyro_straight(600)
|
||||||
|
drive_base.settings(150, 750, 50, 500)
|
||||||
|
await gyro_turn(-30)
|
||||||
|
await gyro_straight(250)
|
||||||
|
#left_arm.run_angle(300,218)
|
||||||
|
|
||||||
|
set_default_speed()
|
||||||
|
await gyro_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 gyro_straight(-600)
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
async def Run12():
|
||||||
|
await gyro_straight(900)
|
||||||
|
await gyro_turn(83)
|
||||||
|
await left_arm.run_angle(3000, -300)
|
||||||
|
await right_arm.run_angle(1100, -180)
|
||||||
|
drive_base.settings(150, 50, 50, 50)
|
||||||
|
await gyro_straight(120)
|
||||||
|
left_arm.reset_angle(0)
|
||||||
|
await left_arm.run_angle(50, 50)
|
||||||
|
await right_arm.run_angle(50, 90)
|
||||||
|
await gyro_straight(-100)
|
||||||
|
drive_base.settings(950, 750, 750, 750)
|
||||||
|
await gyro_turn(110)
|
||||||
|
await gyro_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())
|
||||||
Reference in New Issue
Block a user