"""MicroPython bounded waypoint controller for Pololu 3pi+ 2040. API-checked against official source, NOT physically piloted. Student-measured calibration is required. Do not enable on a table. A arms, C/bump/dark boundary/invalid timing/stall/timeout stops. """ from pololu_3pi_2040_robot import robot from math import atan2, cos, sin, sqrt, pi import time # Enter YOUR measured configuration after completing Weeks 6, 7 and 13. # None deliberately refuses actuation; fictional scales must not move hardware. CONFIG = { "left_m_per_count": None, "right_m_per_count": None, "track_m": None, "floor_dark_threshold": None, # every sensor must be below this on the clear mat "kp_command_per_m_s": None, # selected and validated in Weeks 9-12 "ki_command_per_m": 0.0, # start with P; add I only with evidence "max_command": 600, "max_speed_m_s": 0.06, "max_turn_rad_s": 0.4, "mission_timeout_s": 30.0, "waypoint_timeout_s": 12.0, "arrival_m": 0.03, "heading_rad": 8 * pi / 180, } # Robot starts at surveyed centre (0,0) facing +x, route in metres. WAYPOINTS = [(0.15, 0.0)] def validate(cfg): for key in ("left_m_per_count", "right_m_per_count", "track_m", "kp_command_per_m_s"): value = cfg[key] if value is None or value != value or not 0 < value < 100000: raise ValueError("Measured positive configuration required: " + key) threshold = cfg["floor_dark_threshold"] if threshold is None or not 0 < threshold <= 1024: raise ValueError("Calibrated raw dark boundary threshold required") if not 0 < cfg["max_command"] <= 600: raise ValueError("This introductory controller limits command to 600") if not 0 < cfg["max_speed_m_s"] <= 0.1: raise ValueError("Speed limit must be positive and <= 0.1 m/s") if not 0 < cfg["max_turn_rad_s"] <= 0.5: raise ValueError("Turn limit must be positive and <= 0.5 rad/s") for key in ("mission_timeout_s", "waypoint_timeout_s", "arrival_m", "heading_rad"): if not 0 < cfg[key] <= 60: raise ValueError("Invalid limit: " + key) if not 0 <= cfg["ki_command_per_m"] < 100000: raise ValueError("Invalid integral gain") if not WAYPOINTS: raise ValueError("Empty route") for x, y in WAYPOINTS: if x != x or y != y or abs(x) > 2 or abs(y) > 2: raise ValueError("Invalid introductory waypoint") def clip(value, limit): return max(-limit, min(limit, value)) motors = robot.Motors() motors.off() try: validate(CONFIG) # failure occurs with motors off encoders, lines, bumps = robot.Encoders(), robot.LineSensors(), robot.BumpSensors() button_a, button_c = robot.ButtonA(), robot.ButtonC() bumps.calibrate() print("A arms; C cancels; 30-second arming limit. Clear surveyed mat only.") waiting = time.ticks_ms() while not button_a.is_pressed(): if button_c.is_pressed() or time.ticks_diff(time.ticks_ms(), waiting) >= 30000: raise RuntimeError("Arming cancelled or expired") time.sleep_ms(10) # Check clear floor/contact before any movement. x, y, theta = 0.0, 0.0, 0.0 integral = [0.0, 0.0] previous = encoders.get_counts() started = last = waypoint_started = last_motion = time.ticks_ms() index, reason = 0, "unknown" print("elapsed_s,waypoint,x_m,y_m,theta_rad,left_command,right_command") while True: now = time.ticks_ms() elapsed = time.ticks_diff(now, started) / 1000 dt = time.ticks_diff(now, last) / 1000 values = lines.read() bumps.read() if button_c.is_pressed(): reason = "button_c"; break if bumps.left_is_pressed() or bumps.right_is_pressed(): reason = "contact"; break if len(values) != 5 or any(not 0 <= v <= 1024 for v in values) or max(values) >= CONFIG["floor_dark_threshold"]: reason = "floor_boundary_or_invalid"; break if elapsed >= CONFIG["mission_timeout_s"]: reason = "mission_timeout"; break if time.ticks_diff(now, waypoint_started) / 1000 >= CONFIG["waypoint_timeout_s"]: reason = "waypoint_timeout"; break if dt == 0: time.sleep_ms(20); continue if not 0 < dt <= 0.2: reason = "invalid_timing"; break counts = encoders.get_counts() dl = (counts[0] - previous[0]) * CONFIG["left_m_per_count"] dr = (counts[1] - previous[1]) * CONFIG["right_m_per_count"] # Very loose plausibility bound; not a substitute for actual calibration. if abs(dl) / dt > 0.5 or abs(dr) / dt > 0.5: reason = "implausible_counts"; break if abs(dl) + abs(dr) > 0: last_motion = now elif time.ticks_diff(now, last_motion) > 1500: reason = "encoder_stall"; break ds, turn = (dl + dr) / 2, (dr - dl) / CONFIG["track_m"] x += ds * cos(theta + turn / 2) y += ds * sin(theta + turn / 2) theta = atan2(sin(theta + turn), cos(theta + turn)) previous, last = counts, now gx, gy = WAYPOINTS[index] distance = sqrt((gx - x) ** 2 + (gy - y) ** 2) if distance <= CONFIG["arrival_m"]: motors.off() integral = [0.0, 0.0] index += 1 if index == len(WAYPOINTS): reason = "estimated_arrival"; break waypoint_started = last_motion = now time.sleep_ms(20); continue bearing = atan2(gy - y, gx - x) error_heading = atan2(sin(bearing - theta), cos(bearing - theta)) omega = clip(2 * error_heading, CONFIG["max_turn_rad_s"]) forward = 0.0 if abs(error_heading) > CONFIG["heading_rad"] else min(CONFIG["max_speed_m_s"], distance) targets = [forward - omega * CONFIG["track_m"] / 2, forward + omega * CONFIG["track_m"] / 2] measured = [dl / dt, dr / dt] commands = [] for wheel in range(2): error = targets[wheel] - measured[wheel] candidate_i = clip(integral[wheel] + error * dt, 0.1) raw = CONFIG["kp_command_per_m_s"] * error + CONFIG["ki_command_per_m"] * candidate_i command = clip(raw, CONFIG["max_command"]) # Conditional integration prevents adding error further into saturation. if raw == command or raw * error <= 0: integral[wheel] = candidate_i commands.append(int(command)) motors.set_speeds(commands[0], commands[1]) print(elapsed, index, x, y, theta, commands[0], commands[1], sep=",") time.sleep_ms(20) motors.off() print("stop_reason", reason, "estimated_pose", x, y, theta) finally: motors.off()