# ====== PUCK RETRIEVAL ROBOT - MicroPython (Raspberry Pi Pico) ======
# Components:
# - 2x DC motors via L298N/TB6612 (ENA/ENB PWM, IN1..IN4 DIR)
# - 1x Servo (gripper) on 50 Hz PWM
# - 1x Ultrasonic HC-SR04 (Trig, Echo)
# - 2x LDRs as voltage dividers to ADC0/ADC1
# - State machine: SEARCH → APPROACH → GRASP → RETURN
from machine import Pin, PWM, ADC, time_pulse_us
import utime, math
# ---------------- CONFIG: Pins & Robot Parameters ----------------
# Motor driver pins (edit if needed)
ENA_PIN = 0 # PWM left enable
ENB_PIN = 1 # PWM right enable
IN1_PIN = 2 # Left dir A
IN2_PIN = 3 # Left dir B
IN3_PIN = 4 # Right dir A
IN4_PIN = 5 # Right dir B
# Ultrasonic pins
TRIG_PIN = 6
ECHO_PIN = 7
# Servo pin
SERVO_PIN = 16
# LDR ADC pins (Left/Right)
LDR_L_ADC = 26 # ADC0
LDR_R_ADC = 27 # ADC1
# Physical params (examples; update if you measured differently)
WHEEL_RADIUS_M = 0.03 # 3 cm
TRACK_L_M = 0.10 # 10 cm between wheel centers
V_MAX_MPS = 0.30 # your rough max fwd speed (estimate)
LOOP_DT = 0.05 # 50 ms control loop
# Distance targets (meters)
DIST_TARGET_M = 0.10 # want to stop ~10 cm from puck
GRIP_WINDOW_M = 0.12 # start gripping when closer than ~12 cm
# LDR thresholds (tune in testing)
LDR_SUM_MIN = 22000 # min combined brightness to declare "puck found"
LDR_DIFF_SCALE = 1/32000 # scales ADC diff to ~[-1,1] range
# PID gains (tune!)
# Heading PD (based on LDR difference)
Kp_h = 1.2 # proportional on heading error
Ki_h = 0.0 # usually 0 for heading
Kd_h = 0.15 # derivative for damping
# Distance PID (based on ultrasonic distance error)
Kp_d = 1.0
Ki_d = 0.1
Kd_d = 0.05
V_CMD_CLAMP = 0.18 # clamp forward cmd (m/s) for gentle approach
# Servo angles (deg) -> pulse width mapping
SERVO_OPEN_DEG = 0
SERVO_CLOSED_DEG = 60
SERVO_MIN_US = 500
SERVO_MAX_US = 2500
SERVO_FREQ_HZ = 50
# Motor PWM settings
PWM_FREQ_HZ = 2000
PWM_MIN_START = 18000 # minimal duty_u16 to overcome deadzone (tune)
PWM_MAX = 62000 # near-max; keep headroom
# Return motion config
RETURN_TIME_MS = 2000 # back up for 2 seconds
RETURN_SPEED = 0.12 # m/s
# -------------------- Hardware Abstractions ----------------------
class Motor:
def __init__(self, ena_pin, in_a, in_b, freq=PWM_FREQ_HZ):
[Link] = PWM(Pin(ena_pin))
[Link](freq)
self.in_a = Pin(in_a, [Link])
self.in_b = Pin(in_b, [Link])
def set_speed(self, cmd):
"""
cmd in [-1.0, 1.0]; sign = direction, magnitude = speed
Maps to direction pins + PWM duty_u16
"""
cmd = max(-1.0, min(1.0, cmd))
if cmd >= 0:
self.in_a.value(1)
self.in_b.value(0)
else:
self.in_a.value(0)
self.in_b.value(1)
duty = 0
mag = abs(cmd)
if mag > 0:
duty = int(PWM_MIN_START + mag * (PWM_MAX - PWM_MIN_START))
[Link].duty_u16(duty)
class Servo:
def __init__(self, pin, freq=SERVO_FREQ_HZ):
[Link] = PWM(Pin(pin))
[Link](freq)
def angle(self, deg):
"""Map angle (deg) -> pulse width us -> duty_u16 for 50 Hz."""
deg = max(0, min(180, deg))
pulse = SERVO_MIN_US + (SERVO_MAX_US - SERVO_MIN_US) * (deg / 180.0)
# For 50Hz, period = 20,000 us
duty = int((pulse / 20000.0) * 65535)
[Link].duty_u16(duty)
class Ultrasonic:
def __init__(self, trig_pin, echo_pin):
[Link] = Pin(trig_pin, [Link])
[Link] = Pin(echo_pin, [Link])
[Link](0)
def distance_m(self, timeout_us=25000):
"""Return distance in meters, or None if timeout."""
# Trigger 10 us pulse
[Link](1)
utime.sleep_us(10)
[Link](0)
# Measure echo high time
try:
t = time_pulse_us([Link], 1, timeout_us)
except OSError:
return None
if t < 0: # timeout or invalid
return None
# Speed of sound ~ 343 m/s ⇒ 343 mm/ms ⇒ 0.343 mm/us
# Distance = (t_us * 0.343 mm/us) / 2 = t*0.000343/2 m
return (t * 0.000343) / 2.0
# Simple PID utility
class PID:
def __init__(self, kp, ki, kd, out_min=None, out_max=None):
[Link], [Link], [Link] = kp, ki, kd
self.out_min, self.out_max = out_min, out_max
[Link] = 0.0
self.prev_e = 0.0
[Link] = True
def reset(self):
[Link] = 0.0
self.prev_e = 0.0
[Link] = True
def update(self, e, dt):
[Link] += e * dt
de = 0.0
if not [Link]:
de = (e - self.prev_e) / dt
[Link] = False
u = [Link] * e + [Link] * [Link] + [Link] * de
self.prev_e = e
if self.out_min is not None:
u = max(self.out_min, u)
if self.out_max is not None:
u = min(self.out_max, u)
return u
# ---------------------- Robot Setup ------------------------------
# Motors
motor_left = Motor(ENA_PIN, IN1_PIN, IN2_PIN)
motor_right = Motor(ENB_PIN, IN3_PIN, IN4_PIN)
# Servo
gripper = Servo(SERVO_PIN)
# Sensors
ultra = Ultrasonic(TRIG_PIN, ECHO_PIN)
adc_left = ADC(Pin(LDR_L_ADC))
adc_right = ADC(Pin(LDR_R_ADC))
# ---------------------- Control Helpers --------------------------
def ldr_read():
# Return (left, right, sum, diff)
L = adc_left.read_u16()
R = adc_right.read_u16()
return L, R, (L + R), (L - R)
def heading_pd_from_ldr():
"""
Use LDR difference to compute a heading angular velocity command ω_heading.
Positive diff => brighter on left => turn left (convention).
"""
L, R, S, D = ldr_read()
e_theta = D * LDR_DIFF_SCALE # roughly [-1,1]
# PD control (integral off for heading)
# Derivative via internal state in PID (reuse PID class with Ki=0)
global pid_heading
return pid_heading.update(e_theta, LOOP_DT), S
def distance_pid_from_ultrasonic():
"""
Use ultrasonic to compute forward velocity command v_cmd.
e_d = target - measured; negative => too far, move forward.
"""
d = ultra.distance_m()
if d is None:
# No reliable reading; return small forward command to continue approach
e_d = 0.0
v_cmd = 0.05
return v_cmd, None
e_d = DIST_TARGET_M - d # (m)
v_cmd = pid_distance.update(e_d, LOOP_DT)
# Clamp gentle approach speed
if v_cmd > V_CMD_CLAMP: v_cmd = V_CMD_CLAMP
if v_cmd < -V_CMD_CLAMP: v_cmd = -V_CMD_CLAMP
return v_cmd, d
def vomega_to_wheels(v_cmd, omega_cmd):
"""
Convert commanded (v, ω) to per-wheel linear speeds, then to normalized motor commands [-
1,1].
Without encoders, map speeds to PWM proportionally using V_MAX_MPS.
"""
vR = v_cmd + (TRACK_L_M/2.0)*omega_cmd
vL = v_cmd - (TRACK_L_M/2.0)*omega_cmd
# Normalize by V_MAX_MPS
cmdR = max(-1.0, min(1.0, vR / V_MAX_MPS))
cmdL = max(-1.0, min(1.0, vL / V_MAX_MPS))
return cmdL, cmdR
def stop_motors():
motor_left.set_speed(0.0)
motor_right.set_speed(0.0)
def rotate_in_place(speed=0.15, left_ccw=True):
"""Rotate on spot by driving wheels opposite directions."""
s = max(0.0, min(0.4, speed)) # keep modest
if left_ccw:
motor_left.set_speed(-s)
motor_right.set_speed(s)
else:
motor_left.set_speed(s)
motor_right.set_speed(-s)
def gripper_open():
[Link](SERVO_OPEN_DEG)
def gripper_close():
[Link](SERVO_CLOSED_DEG)
# ---------------------- PID controllers --------------------------
pid_heading = PID(Kp_h, Ki_h, Kd_h, out_min=-2.5, out_max=2.5) # ω_heading clamp (rad/s)
pid_distance = PID(Kp_d, Ki_d, Kd_d, out_min=-0.25, out_max=0.25) # v_cmd clamp (m/s)
# ---------------------- State Machine ----------------------------
STATE_SEARCH = 0
STATE_APPROACH = 1
STATE_GRASP = 2
STATE_RETURN = 3
state = STATE_SEARCH
gripper_open()
last = utime.ticks_ms()
return_start_ms = None
print("Robot starting. State = SEARCH")
while True:
now = utime.ticks_ms()
if utime.ticks_diff(now, last) < int(LOOP_DT*1000):
utime.sleep_ms(1)
continue
last = now
if state == STATE_SEARCH:
# Rotate gently until we see enough light
rotate_in_place(speed=0.12, left_ccw=True)
_, _, S, _ = ldr_read()
# Optional: small random dither after N seconds
if S >= LDR_SUM_MIN:
stop_motors()
pid_heading.reset()
pid_distance.reset()
state = STATE_APPROACH
print("→ APPROACH (light found)")
elif state == STATE_APPROACH:
omega_cmd, S = heading_pd_from_ldr() # turn rate from LDR diff
v_cmd, d = distance_pid_from_ultrasonic() # fwd speed from distance
# If no light anymore, fall back to SEARCH
if S < LDR_SUM_MIN:
stop_motors()
state = STATE_SEARCH
print("↩ SEARCH (lost light)")
continue
# Safety: if ultrasonic invalid for a while, keep creeping forward slowly
if d is not None and d <= GRIP_WINDOW_M:
stop_motors()
state = STATE_GRASP
print("→ GRASP (in range)")
continue
# Drive
cmdL, cmdR = vomega_to_wheels(v_cmd, omega_cmd)
motor_left.set_speed(cmdL)
motor_right.set_speed(cmdR)
elif state == STATE_GRASP:
stop_motors()
gripper_close()
utime.sleep_ms(600) # allow servo to finish
# Optional: verify grip via small distance change or a delay
return_start_ms = utime.ticks_ms()
state = STATE_RETURN
print("→ RETURN")
elif state == STATE_RETURN:
# Back straight for a fixed time, then release and go to SEARCH
elapsed = utime.ticks_diff(utime.ticks_ms(), return_start_ms)
if elapsed < RETURN_TIME_MS:
# Backward at RETURN_SPEED
v_cmd = -RETURN_SPEED
omega_cmd = 0.0
cmdL, cmdR = vomega_to_wheels(v_cmd, omega_cmd)
motor_left.set_speed(cmdL)
motor_right.set_speed(cmdR)
else:
stop_motors()
gripper_open()
utime.sleep_ms(400)
state = STATE_SEARCH
print("↩ SEARCH (cycle complete)")