From 8871e16b4eda6a41376d10921b9988205a1926b0 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Fri, 15 May 2026 12:49:43 +0000 Subject: [PATCH] Update competition_codes/AROC/The_South-Awakens.py Yes, it has an extra 500 lines of code, but its APID. --- competition_codes/AROC/The_South-Awakens.py | 1032 ++++++++++++------- 1 file changed, 652 insertions(+), 380 deletions(-) diff --git a/competition_codes/AROC/The_South-Awakens.py b/competition_codes/AROC/The_South-Awakens.py index 93b3e4e..fba12b4 100644 --- a/competition_codes/AROC/The_South-Awakens.py +++ b/competition_codes/AROC/The_South-Awakens.py @@ -10,282 +10,585 @@ 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 +right_motor = Motor(Port.B, Direction.CLOCKWISE) +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12, 36]], [[12, 20, 24]]) +right_arm = Motor(Port.D, Direction.CLOCKWISE, [[12, 36], [12, 20, 24]]) 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) +WHEEL_DIAMETER = 68.8 # mm +AXLE_TRACK = 180 # mm drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) drive_base.settings(600, 500, 300, 200) drive_base.use_gyro(True) +# ============================================================ +# ADVANCED PID MODULE +# ============================================================ + +class PIDController: + """ + Advanced PID Controller with: + - Anti-windup (integral clamping) + - Derivative filtering (low-pass) + - Deadband tolerance + - Output saturation + - Setpoint ramping + - Error history tracking for diagnostics + """ + + def __init__( + self, + kp: float, + ki: float, + kd: float, + setpoint: float = 0.0, + output_min: float = -100.0, + output_max: float = 100.0, + integral_limit: float = 1000.0, + deadband: float = 0.5, + derivative_filter: float = 0.2, + name: str = "PID", + ): + self.kp = kp + self.ki = ki + self.kd = kd + self.setpoint = setpoint + self.output_min = output_min + self.output_max = output_max + self.integral_limit = integral_limit + self.deadband = deadband + self.derivative_filter = derivative_filter + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._timer = StopWatch() + self._first_update = True + self.name = name + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + self._max_history = 50 + + def reset(self): + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._first_update = True + self._timer.reset() + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + + def set_setpoint(self, setpoint: float): + self.setpoint = setpoint + + def set_gains(self, kp: float, ki: float, kd: float): + self.kp = kp + self.ki = ki + self.kd = kd + + def _clamp(self, value: float, low: float, high: float) -> float: + if value < low: + return low + if value > high: + return high + return value + + def update(self, current_value: float) -> float: + dt_ms = self._timer.time() + self._timer.reset() + + if self._first_update or dt_ms <= 0: + dt = 0.02 + self._first_update = False + else: + dt = dt_ms / 1000.0 + + error = self.setpoint - current_value + + if abs(error) < self.deadband: + error = 0.0 + + p_term = self.kp * error + + self._integral += error * dt + self._integral = self._clamp( + self._integral, -self.integral_limit, self.integral_limit + ) + i_term = self.ki * self._integral + + if dt > 0: + raw_derivative = (error - self._prev_error) / dt + else: + raw_derivative = 0.0 + + alpha = self.derivative_filter + filtered_derivative = ( + alpha * self._prev_derivative + (1.0 - alpha) * raw_derivative + ) + d_term = self.kd * filtered_derivative + + output = p_term + i_term + d_term + output = self._clamp(output, self.output_min, self.output_max) + + self._prev_error = error + self._prev_derivative = filtered_derivative + self._last_output = output + self._last_p = p_term + self._last_i = i_term + self._last_d = d_term + + if len(self._error_history) >= self._max_history: + self._error_history.pop(0) + self._error_history.append(error) + + return output + + def is_settled(self, tolerance: float = 1.0, samples: int = 5) -> bool: + if len(self._error_history) < samples: + return False + recent = self._error_history[-samples:] + return all(abs(e) < tolerance for e in recent) + + def diagnostics(self) -> str: + return ( + f"[{self.name}] P={self._last_p:.2f} I={self._last_i:.2f} " + f"D={self._last_d:.2f} Out={self._last_output:.2f} " + f"Err={self._prev_error:.2f}" + ) + + +# ============================================================ +# PID-BASED DRIVE VARIABLE +# ============================================================ + +class PIDDrive: + """ + High-level PID driving interface that wraps the gyro and motors. + """ + + def __init__( + self, + hub_ref, + left: Motor, + right: Motor, + drivebase: DriveBase, + ultrasonic: UltrasonicSensor, + wheel_diameter: float = 68.8, + axle_track: float = 180.0, + ): + self.hub = hub_ref + self.left = left + self.right = right + self.db = drivebase + self.ultrasonic = ultrasonic + self.wheel_diameter = wheel_diameter + self.axle_track = axle_track + self.mm_per_degree = (umath.pi * wheel_diameter) / 360.0 + + self.straight_pid = PIDController( + kp=3.0, ki=0.05, kd=1.5, + output_min=-150, output_max=150, + integral_limit=500, deadband=0.3, + derivative_filter=0.3, name="Straight", + ) + + self.turn_pid = PIDController( + kp=4.0, ki=0.1, kd=2.0, + output_min=-400, output_max=400, + integral_limit=800, deadband=0.5, + derivative_filter=0.25, name="Turn", + ) + + self.distance_pid = PIDController( + kp=1.5, ki=0.02, kd=1.0, + output_min=-300, output_max=300, + integral_limit=500, deadband=2.0, + derivative_filter=0.3, name="Distance", + ) + + self.accel_rate = 5 + self.decel_distance = 80 + + def _get_heading(self) -> float: + return self.hub.imu.heading() + + def _reset_heading(self): + self.hub.imu.reset_heading(0) + + def _reset_motors(self): + self.left.reset_angle(0) + self.right.reset_angle(0) + + def _get_distance_traveled(self) -> float: + left_deg = abs(self.left.angle()) + right_deg = abs(self.right.angle()) + avg_deg = (left_deg + right_deg) / 2.0 + return avg_deg * self.mm_per_degree + + def _drive_motors(self, left_speed: float, right_speed: float): + self.left.run(left_speed) + self.right.run(right_speed) + + def _stop_motors(self, stop_type=Stop.BRAKE): + self.left.stop() + self.right.stop() + if stop_type == Stop.HOLD: + self.left.hold() + self.right.hold() + + async def pid_straight( + self, + distance_mm: float, + base_speed: float = 400, + settle_tolerance: float = 2.0, + settle_samples: int = 3, + timeout_ms: int = 8000, + ): + self._reset_heading() + self._reset_motors() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + direction = 1 if distance_mm >= 0 else -1 + target_dist = abs(distance_mm) + timer = StopWatch() + + while True: + traveled = self._get_distance_traveled() + remaining = target_dist - traveled + + if remaining <= 0: + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Straight] Timeout after {timeout_ms}ms") + break + + ramp_up_speed = min(base_speed, 100 + traveled * self.accel_rate) + if remaining < self.decel_distance: + decel_speed = max(80, base_speed * (remaining / self.decel_distance)) + else: + decel_speed = base_speed + speed = min(ramp_up_speed, decel_speed) + + current_heading = self._get_heading() + correction = self.straight_pid.update(current_heading) + + left_speed = direction * speed + correction + right_speed = direction * speed - correction + + self._drive_motors(left_speed, right_speed) + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Straight] dist={traveled:.0f}/{target_dist:.0f} " + f"spd={speed:.0f} {self.straight_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + if DEBUG: + print( + f"[PID Straight] Complete: {self._get_distance_traveled():.1f}mm " + f"of {distance_mm}mm" + ) + + async def pid_turn( + self, + angle_deg: float, + max_speed: float = 300, + settle_tolerance: float = 1.0, + settle_samples: int = 5, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.turn_pid.reset() + self.turn_pid.set_setpoint(angle_deg) + + timer = StopWatch() + + while True: + current_heading = self._get_heading() + output = self.turn_pid.update(current_heading) + + if output > max_speed: + output = max_speed + elif output < -max_speed: + output = -max_speed + + if abs(output) < 20 and abs(angle_deg - current_heading) > settle_tolerance: + if output > 0: + output = 20 + elif output < 0: + output = -20 + + self._drive_motors(output, -output) + + if self.turn_pid.is_settled(settle_tolerance, settle_samples): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Turn] Timeout after {timeout_ms}ms") + break + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Turn] heading={current_heading:.1f}/{angle_deg:.1f} " + f"{self.turn_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + final = self._get_heading() + print( + f"[PID Turn] Complete: {final:.1f}° of {angle_deg:.1f}° " + f"(error={angle_deg - final:.2f}°)" + ) + + async def pid_drive_until( + self, + base_speed: float = 300, + stop_distance_mm: float = 100, + timeout_ms: int = 10000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is not None and distance < stop_distance_mm: + if DEBUG: + print(f"[PID DriveUntil] Object at {distance}mm — stopping") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID DriveUntil] Timeout") + break + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + if distance is not None and distance < stop_distance_mm * 2: + speed_factor = distance / (stop_distance_mm * 2) + speed = max(80, base_speed * speed_factor) + else: + speed = base_speed + + self._drive_motors(speed + correction, speed - correction) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + + async def pid_align_to_wall( + self, + target_distance_mm: float = 100, + base_speed: float = 200, + tolerance: float = 3.0, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + self.distance_pid.reset() + self.distance_pid.set_setpoint(target_distance_mm) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is None: + await wait(20) + continue + + speed = -self.distance_pid.update(distance) + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + self._drive_motors(speed + correction, speed - correction) + + if self.distance_pid.is_settled(tolerance, 5): + if DEBUG: + print(f"[PID Align] Settled at {distance}mm") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Align] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + + async def pid_arc( + self, + radius_mm: float, + angle_deg: float, + base_speed: float = 300, + timeout_ms: int = 8000, + ): + self._reset_heading() + self.turn_pid.reset() + self._reset_motors() + + if radius_mm == 0: + await self.pid_turn(angle_deg, base_speed) + return + + abs_radius = abs(radius_mm) + outer_radius = abs_radius + (self.axle_track / 2.0) + inner_radius = abs_radius - (self.axle_track / 2.0) + + if inner_radius < 0: + inner_radius = 0 + + speed_ratio = inner_radius / outer_radius if outer_radius > 0 else 0 + + target_heading = angle_deg + self.turn_pid.set_setpoint(target_heading) + + direction = 1 if angle_deg >= 0 else -1 + timer = StopWatch() + + while True: + heading = self._get_heading() + correction = self.turn_pid.update(heading) + + if abs(target_heading) > 0: + progress = abs(heading) / abs(target_heading) + else: + progress = 1.0 + + if progress > 0.8: + speed = max(60, base_speed * (1.0 - progress) * 5) + else: + speed = base_speed + + outer_speed = direction * speed + inner_speed = direction * speed * speed_ratio + + if radius_mm > 0: + left_speed = outer_speed + correction * 0.3 + right_speed = inner_speed - correction * 0.3 + else: + left_speed = inner_speed + correction * 0.3 + right_speed = outer_speed - correction * 0.3 + + self._drive_motors(left_speed, right_speed) + + if self.turn_pid.is_settled(1.5, 4): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Arc] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + print(f"[PID Arc] Final heading: {self._get_heading():.1f}° / {angle_deg}°") + + +# ============================================================ +# INSTANTIATE THE PID DRIVE VARIABLE +# ============================================================ + +drive = PIDDrive( + hub_ref=hub, + left=left_motor, + right=right_motor, + drivebase=drive_base, + ultrasonic=lazer_ranger, + wheel_diameter=WHEEL_DIAMETER, + axle_track=AXLE_TRACK, +) + +# ============================================================ +# TUNING PRESETS +# ============================================================ + +def set_precise_mode(): + drive.straight_pid.set_gains(kp=4.0, ki=0.08, kd=2.0) + drive.turn_pid.set_gains(kp=5.0, ki=0.15, kd=2.5) + drive.accel_rate = 3 + drive.decel_distance = 100 + +def set_fast_mode(): + drive.straight_pid.set_gains(kp=2.5, ki=0.03, kd=1.0) + drive.turn_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.accel_rate = 8 + drive.decel_distance = 60 + +def set_default_pid_mode(): + drive.straight_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.turn_pid.set_gains(kp=4.0, ki=0.1, kd=2.0) + drive.accel_rate = 5 + drive.decel_distance = 80 + + """ 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 +DEBUG = 1 +async def get_battery_percentage(voltage_mV: float): + max_voltage = 8400.0 + min_voltage = 5000.0 + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage)) * 100 + return max(0, min(100, percentage)) async def wait_button_release(): - """Wait for all buttons to be released""" while hub.buttons.pressed(): await wait(500) - await wait(1000) # Debounce delay + await wait(1000) + +WALL_DISTANCE = 300 -WALL_DISTANCE = 300 # mm async def drive_forward(): - """Drive forward continuously using DriveBase.""" - drive_base.drive(1000,0) + 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) - + 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) + 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 @@ -293,75 +596,59 @@ Run#1 - 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) +async def Run1(): + await left_arm.run_angle(2000, -210) 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) + 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) + right_arm.run_angle(500, 30) + await left_arm.run_angle(500, 75, Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") - - await drive_base.straight(190) + await drive_base.straight(190) await drive_base.turn(-40) await drive_base.straight(335) - await left_arm.run_angle(500,-20) + 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 left_arm.run_angle(500, 50) await drive_base.straight(-100) - left_arm.run_angle(500,-50) + left_arm.run_angle(500, -50) await drive_base.turn(-20) - left_arm.run_angle(1000,180) + 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 + right_arm.run_angle(500, 30) 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.straight(180) await drive_base.turn(-40) await drive_base.straight(335) - await left_arm.run_angle(500,-20) + 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 left_arm.run_angle(500, 50) await drive_base.straight(-100) - left_arm.run_angle(500,-50) + left_arm.run_angle(500, -50) await drive_base.turn(-20) - left_arm.run_angle(1000,180) + left_arm.run_angle(1000, 180) await drive_base.turn(15) async def solve_silo(): @@ -369,17 +656,14 @@ async def solve_silo(): 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 + SPEED = 10000 + SWING_ANGLE = 60 - # 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 - + await right_arm.run_angle(SPEED, SWING_ANGLE, Stop.HOLD) + await right_arm.run_angle(SPEED, (-1 * SWING_ANGLE), Stop.HOLD) - right_arm.run_angle(4000,60, Stop.HOLD) + right_arm.run_angle(4000, 60, Stop.HOLD) """ @@ -387,7 +671,7 @@ Run#2 - This to solve forge, who lived here and heavy lifting combined - Red Key """ -async def Run2(): +async def Run2(): await solve_forge() await solve_heavy_lifting() await solve_who_lived_here() @@ -401,10 +685,9 @@ async def Run2(): drive_base.stop() async def solve_forge(): - left_arm.run_angle(100,90) - await right_arm.run_target(50,50) + 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) @@ -415,11 +698,11 @@ async def solve_forge(): await drive_base.straight(-60) async def solve_heavy_lifting(): - await right_arm.run_angle(2000,-160) # arm down + await right_arm.run_angle(2000, -160) 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 + await drive_base.turn(30) + await right_arm.run_angle(2000, 160) + await drive_base.turn(-30) async def solve_who_lived_here(): await drive_base.straight(35) @@ -433,40 +716,36 @@ async def solve_who_lived_here(): async def solve_flag(): await drive_base.straight(85) - await left_arm.run_angle(70,-90) + await left_arm.run_angle(70, -90) await wait(500) - await left_arm.run_angle(100,120) + 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) + 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(): +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) + 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 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 @@ -475,8 +754,7 @@ Run#3 async def Run3(): await solve_tip_the_scale() await solve_angler_artifacts() - - #cross over to red side + await multitask( drive_forward(), monitor_distance() @@ -484,7 +762,7 @@ async def Run3(): async def solve_tip_the_scale(): drive_base.straight(20) - await drive_base.arc(-275,None,365) + await drive_base.arc(-275, None, 365) await drive_base.straight(280) await drive_base.straight(-80) await drive_base.turn(-50) @@ -496,18 +774,14 @@ async def solve_tip_the_scale(): async def solve_angler_artifacts(): await drive_base.straight(55) await drive_base.turn(-10) - await left_arm.run_angle(10000,-800) + 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(): @@ -535,40 +809,44 @@ 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 +- *** USES PID DRIVE *** """ async def Run5(): # Getting the sand down - await gyro_straight(550) - await right_arm.run_angle(300,100) - await gyro_straight(-75) + await drive.pid_straight(550) + await right_arm.run_angle(300, 100) + await drive.pid_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) + await drive.pid_straight(300) + await drive.pid_straight(-200) + await drive.pid_turn(-15) # Solving statue - await gyro_straight(350) - await gyro_turn(-104) - await gyro_straight(-80) + await drive.pid_straight(350) + await drive.pid_turn(-104) + await drive.pid_straight(-80) await left_arm.run_angle(500, -300) - await gyro_straight(120) - await gyro_turn(5) + await drive.pid_straight(120) + await drive.pid_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) + await drive.pid_turn(18) + await drive.pid_straight(-100) + await drive.pid_turn(-90) + set_fast_mode() + await drive.pid_straight(900) + set_default_pid_mode() drive_base.stop() async def solve_salvage_operation(): await drive_base.straight(500) - await right_arm.run_angle(300,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) @@ -584,19 +862,17 @@ async def solve_statue_rebuild(): 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 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 +async def Run6_7(): solve_site_mark_1() solve_site_mark_2() - #return to base await drive_base.straight(-300) drive_base.stop() @@ -612,76 +888,80 @@ async def solve_site_mark_2(): await wait(50) await right_arm.run_angle(50, 50) -async def Run10(): # experimental map reveal attachment - +async def Run10(): 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) - + 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 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) + +""" +Run#11 +- *** USES PID DRIVE *** +""" +async def Run11(): + await drive.pid_straight(600) + set_precise_mode() + await drive.pid_turn(-30) + await drive.pid_straight(250) + + set_default_pid_mode() + await drive.pid_straight(-100) + await drive.pid_turn(30) + await drive.pid_straight(-600) drive_base.stop() + +""" +Run#12 +- *** USES PID DRIVE *** +""" async def Run12(): - await gyro_straight(900) - await gyro_turn(83) + set_fast_mode() + await drive.pid_straight(900) + await drive.pid_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) + set_precise_mode() + await drive.pid_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) + await drive.pid_straight(-100) + set_fast_mode() + await drive.pid_turn(110) + await drive.pid_straight(1000) + set_default_pid_mode() # Function to classify color based on HSV def detect_color(h, s, v, reflected): if reflected > 4: - if h < 4 or h > 350: # red + if h < 4 or h > 350: return "Red" - elif 3 < h < 40 and s > 70: # orange + elif 3 < h < 40 and s > 70: return "Orange" - elif 47 < h < 56: # yellow + elif 47 < h < 56: return "Yellow" - elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + elif 70 < h < 160: return "Green" - elif 195 < h < 198: # light blue + elif 195 < h < 198: return "Light_Blue" - elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + elif 210 < h < 225: return "Blue" - elif 260 < h < 350: # purple + elif 260 < h < 350: return "Purple" - else: return "Unknown" return "Unknown" @@ -689,16 +969,12 @@ def detect_color(h, s, v, reflected): async def main(): while True: - pressed = hub.buttons.pressed() + 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) + if DEBUG: print(f"button pressed: {pressed}") - if color == "Green": print('Running Mission 1') @@ -717,19 +993,15 @@ async def main(): await Run5() elif color == "Purple": print('Running Mission 6') - await Run11() + 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) + if Button.BLUETOOTH in pressed: 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") @@ -741,5 +1013,5 @@ async def main(): continue await wait(10) -# Run the main function + run_task(main()) \ No newline at end of file