Project 6.7: "Internal Positioning System"

What you’ll learn

• ✅ Odometry fundamentals: Estimate position using wheel movement (simulated encoders) and heading.
• ✅ Drift correction: Use accelerometer tilt to gently correct heading drift over time.
• ✅ Simple mapping: Record visited points on a tiny grid and mark obstacles.
• ✅ Go‑to navigation: Compute a vector to a target and step toward it safely.
• ✅ Trajectory correction: Detect deviation from the planned path and nudge back.

Blocks glossary (used in this project)

• Wheel ticks (simulated): Integer counts representing wheel steps for left/right.
• Pose variables: x, y, theta track position (cm) and heading (radians).
• Odometry update: Convert ticks to distance and update pose using cos/sin.
• Accelerometer bias: Small heading correction from tilt/acc estimates.
• Grid map: Store “visited” and “blocked” cells in a simple set.
• Serial println: Print “POSE:…”, “MAP:…”, “NAV:…”, and “CORR:…” for clarity.

What you need

Notes

• Encoders are simulated with “ticks” per movement pulse; you can later replace with real encoders.
• Share ground across all modules; keep I2C wires short to reduce noise.

Before you start

• Hardware wired and grounded
• Serial monitor open
• Quick test:

print("Ready!")  # Confirm serial is working so you can see messages

Microprojects 1–5

Microproject 6.7.1 – Position estimation by odometry

Goal: Track x, y, theta using simulated wheel ticks per move pulse.
Blocks used:

• Odometry math: distance = ticks * cm_per_tick; heading changes from differential ticks.
• Serial println: Print POSE after each update.

MicroPython code:

import math  # Import math for sin, cos, and pi calculations

# Robot geometry and simulation constants
cm_per_tick = 0.8  # Define centimeters per tick (calibrate for your robot)
wheel_base_cm = 10.0  # Define distance between wheels in centimeters
print("ODO:CONST cm_per_tick", cm_per_tick, "wheel_base_cm", wheel_base_cm)  # Print constants

# Pose state (x, y in cm; theta in radians)
x = 0.0  # Initialize x position to 0 cm
y = 0.0  # Initialize y position to 0 cm
theta = 0.0  # Initialize heading theta to 0 radians (facing +X)
print("POSE:INIT", x, y, theta)  # Print initial pose

def odo_update(ticks_left, ticks_right):  # Define odometry update from wheel ticks
    global x, y, theta  # Use global pose variables
    dl = ticks_left * cm_per_tick  # Convert left ticks to distance in cm
    dr = ticks_right * cm_per_tick  # Convert right ticks to distance in cm
    dc = (dl + dr) / 2.0  # Compute center distance moved
    dtheta = (dr - dl) / wheel_base_cm  # Compute heading change in radians
    theta += dtheta  # Update heading with delta
    x += dc * math.cos(theta)  # Update x using forward projection
    y += dc * math.sin(theta)  # Update y using forward projection
    print("POSE:UPDATE", round(x, 2), round(y, 2), round(theta, 3))  # Print updated pose

# Simulated moves: forward, right turn, forward
odo_update(5, 5)  # Simulate 5 left ticks and 5 right ticks (straight forward)
odo_update(2, 4)  # Simulate differential ticks (turn slightly right while moving)
odo_update(6, 6)  # Simulate straight forward again

Reflection: Odometry is your robot’s internal map—no GPS, just wheels and math.
Challenge:

• Easy: Make a helper “odo_forward(ticks)” that calls odo_update(t, t).
• Harder: Add “odo_rotate(ticks_left, ticks_right)” to rotate in place.

Microproject 6.7.2 – Drift correction with accelerometer

Goal: Read ADXL345 and apply a tiny heading correction when tilt suggests deviation.
Blocks used:

• I2C read: Get X/Y/Z signed counts.
• Bias rule: Map a small Y tilt to heading correction.

MicroPython code:

import machine  # Import machine for I2C pins
import time  # Import time for delays

# ADXL345 registers and address
ADXL_ADDR = 0x53  # Set accelerometer I2C address to 0x53
REG_POWER_CTL = 0x2D  # Set power control register address
REG_DATA_FORMAT = 0x31  # Set data format register address
REG_DATAX0 = 0x32  # Set first axis data register address

# I2C setup on pins 22/21
i2c = machine.SoftI2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=100000)  # Create I2C bus
i2c.writeto_mem(ADXL_ADDR, REG_POWER_CTL, b'\x08')  # Enable measurement mode
i2c.writeto_mem(ADXL_ADDR, REG_DATA_FORMAT, b'\x00')  # Set ±2g default range

def read_xyz():  # Define helper to read accelerometer axes
    buf = i2c.readfrom_mem(ADXL_ADDR, REG_DATAX0, 6)  # Read 6 bytes for X,Y,Z
    x_a = int.from_bytes(buf[0:2], 'little', signed=True)  # Convert X bytes to signed int
    y_a = int.from_bytes(buf[2:4], 'little', signed=True)  # Convert Y bytes to signed int
    z_a = int.from_bytes(buf[4:6], 'little', signed=True)  # Convert Z bytes to signed int
    return x_a, y_a, z_a  # Return axes

corr_gain = 0.0008  # Define small gain to convert tilt to heading correction
print("CORR:GAIN", corr_gain)  # Print correction gain

def apply_drift_correction():  # Define function to correct theta using accelerometer
    global theta  # Use global heading variable
    x_a, y_a, z_a = read_xyz()  # Read accelerometer axes
    theta += y_a * corr_gain  # Apply small correction proportional to Y tilt
    print("CORR:APPLY", y_a, round(theta, 3))  # Print Y tilt and new heading

apply_drift_correction()  # Call correction once
time.sleep(0.1)  # Short delay for readability
apply_drift_correction()  # Call correction a second time

Reflection: Accelerometer adds “feel”—a gentle nudge against heading drift.
Challenge:

• Easy: Average 3 reads before applying correction.
• Harder: Clamp correction per step to ±0.01 radians for stability.

Microproject 6.7.3 – Simple environment mapping

Goal: Store visited points on a tiny grid and mark obstacles using ultrasonic hints.
Blocks used:

• Grid rounding: Convert x, y to integer cells.
• Sets: Keep visited and blocked tags.

MicroPython code:

# Optional ultrasonic pins (if available)
trig = machine.Pin(27, machine.Pin.OUT)  # Create TRIG pin on 27
echo = machine.Pin(25, machine.Pin.IN)  # Create ECHO pin on 25
print("ULTRA:READY TRIG=27 ECHO=25")  # Confirm ultrasonic pins

def ultra_cm():  # Define ultrasonic distance measurement
    trig.value(0)  # Start with trigger LOW
    time.sleep_us(3)  # Short settle time
    trig.value(1)  # Set trigger HIGH
    time.sleep_us(10)  # Keep HIGH for 10 microseconds
    trig.value(0)  # Set trigger LOW
    t0 = time.ticks_us()  # Record start time for timeout
    while echo.value() == 0:  # Wait for rising edge of echo
        if time.ticks_diff(time.ticks_us(), t0) > 20000:  # If wait exceeds 20 ms
            return 999  # Return large value to indicate no echo
    start = time.ticks_us()  # Record pulse start time
    while echo.value() == 1:  # Wait while echo stays HIGH
        pass  # Busy-wait until pulse ends
    end = time.ticks_us()  # Record pulse end time
    dur = time.ticks_diff(end, start)  # Compute duration in microseconds
    return dur // 58  # Convert microseconds to centimeters

visited = set()  # Initialize set for visited grid cells
blocked = set()  # Initialize set for blocked grid cells
grid_cm = 5  # Define grid cell size in centimeters
print("MAP:GRID", grid_cm)  # Print grid cell size

def cell_from_pose():  # Define helper to convert pose to grid cell
    cx = int(round(x / grid_cm))  # Compute grid x index from x cm
    cy = int(round(y / grid_cm))  # Compute grid y index from y cm
    return (cx, cy)  # Return cell tuple

def mark_map():  # Define function to mark visited and blocked cells
    cell = cell_from_pose()  # Compute current cell
    visited.add(cell)  # Add current cell to visited set
    cm = ultra_cm()  # Measure front distance centimeters
    if cm <= 20:  # If object within 20 cm
        ahead_cell = (cell[0] + 1, cell[1])  # Assume facing +X and mark cell ahead
        blocked.add(ahead_cell)  # Add ahead cell to blocked set
    print("MAP:VISITED", list(visited))  # Print visited cells
    print("MAP:BLOCKED", list(blocked))  # Print blocked cells

mark_map()  # Mark map once at current pose
odo_update(5, 5)  # Move forward in simulation
mark_map()  # Mark map again after move

Reflection: A tiny grid gives the robot memory—“I was here; that cell ahead might be blocked.”
Challenge:

• Easy: Mark left/right neighbor cells when rotating.
• Harder: Store a “timestamp” alongside each visited cell to age data.

Microproject 6.7.4 – Navigation to a specific point

Goal: Compute a vector to a target and step toward it using odometry and heading.
Blocks used:

• Vector math: target_dx, target_dy, target_angle, bearing error.
• Step policy: Turn toward target, then move forward pulses.

MicroPython code:

# Target coordinates (cm) in world frame
tx, ty = 30.0, 20.0  # Set target x and y to 30 cm and 20 cm
print("NAV:TARGET", tx, ty)  # Print target coordinates

def angle_wrap(a):  # Define helper to wrap angle to [-pi, pi]
    while a > math.pi:  # If angle exceeds +pi
        a -= 2 * math.pi  # Subtract full circle
    while a < -math.pi:  # If angle less than -pi
        a += 2 * math.pi  # Add full circle
    return a  # Return wrapped angle

def nav_step():  # Define one navigation step toward target
    global theta  # Use global heading variable
    dx = tx - x  # Compute delta x to target
    dy = ty - y  # Compute delta y to target
    dist = math.hypot(dx, dy)  # Compute Euclidean distance
    target_angle = math.atan2(dy, dx)  # Compute angle to target
    err = angle_wrap(target_angle - theta)  # Compute bearing error
    print("NAV:ERR", round(err, 3), "DIST", round(dist, 2))  # Print error and distance
    if abs(err) > 0.2:  # If heading error is large
        dl = 2  # Define left ticks for rotation
        dr = 0  # Define right ticks for rotation
        odo_update(dl, dr)  # Update pose with rotation
    elif dist > 2.0:  # If far from target
        odo_update(3, 3)  # Update pose with forward motion
    else:  # If close enough
        print("NAV:ARRIVED")  # Print arrival message

# Run several navigation steps
for i in range(10):  # Iterate 10 steps
    nav_step()  # Perform one navigation step

Reflection: “Face it, then go” is the simplest path strategy—and it works surprisingly well.
Challenge:

• Easy: Increase forward ticks when dist > 10 cm.
• Harder: Reduce turn ticks as you get close to the target for smooth arrival.

Microproject 6.7.5 – Trajectory correction

Goal: Detect deviation from the planned line and nudge back toward the vector.
Blocks used:

• Cross‑track error: Perpendicular distance from current pose to line.
• Correction nudge: Small rotation or forward bias to reduce error.

MicroPython code:

# Planned path line from start (0,0) to target (tx, ty)
print("TRAJ:LINE 0,0 ->", tx, ty)  # Print planned line endpoints

def cross_track_error(px, py, ax, ay, bx, by):  # Define cross-track error function
    vx = bx - ax  # Compute x component of line vector
    vy = by - ay  # Compute y component of line vector
    wx = px - ax  # Compute x component from A to point P
    wy = py - ay  # Compute y component from A to point P
    line_len = math.hypot(vx, vy)  # Compute line length
    if line_len == 0:  # If line length is zero
        return 0.0  # Return zero error
    # Signed cross-track: perpendicular projection magnitude with sign
    sign = 1.0 if (vx * wy - vy * wx) > 0 else -1.0  # Determine side of line
    err = sign * abs(vx * wy - vy * wx) / line_len  # Compute signed perpendicular distance
    return err  # Return cross-track error

def correction_step():  # Define one trajectory correction step
    err = cross_track_error(x, y, 0.0, 0.0, tx, ty)  # Compute cross-track error to target line
    print("CORR:CTE", round(err, 2))  # Print cross-track error
    if abs(err) > 2.0:  # If deviation is larger than 2 cm
        # Nudge heading opposite the error sign
        if err > 0:  # If error is positive
            odo_update(2, 3)  # Apply slight right bias to correct left deviation
        else:  # If error is negative
            odo_update(3, 2)  # Apply slight left bias to correct right deviation
    else:  # If deviation is small
        odo_update(3, 3)  # Move forward toward the line
    print("POSE:", round(x, 1), round(y, 1), round(theta, 2))  # Print pose after correction

# Run correction steps alongside nav
for i in range(8):  # Iterate correction sequence
    correction_step()  # Perform a correction step

Reflection: Tiny nudges keep you glued to the path—like balancing on a beam.
Challenge:

• Easy: Lower the error threshold to 1.5 cm for sharper tracking.
• Harder: Scale the nudge by error magnitude (bigger deviation, stronger correction).

Main project – Internal positioning system

Blocks steps (with glossary)

• Odometry core: Update pose from left/right ticks and heading math.
• Drift correction: Read accelerometer and apply small heading bias.
• Grid mapping: Mark visited cells and blocked neighbors using distance hints.
• Go‑to target: Turn toward target, step forward, and print arrival.
• Trajectory correction: Compute cross‑track error and nudge back toward the line.

MicroPython code (mirroring blocks)

# Project 6.7 – Internal Positioning System (Odometry + Drift + Mapping + Navigation)

import machine  # Import machine for I2C pins and optional ultrasonic
import math  # Import math for trigonometry
import time  # Import time for delays

# Geometry and simulation constants
cm_per_tick = 0.8  # Define centimeters per wheel tick
wheel_base_cm = 10.0  # Define wheel base width in centimeters
print("INIT:CONST", cm_per_tick, wheel_base_cm)  # Print constants

# Pose state
x = 0.0  # Initialize x position (cm)
y = 0.0  # Initialize y position (cm)
theta = 0.0  # Initialize heading (radians)
print("POSE:INIT", x, y, theta)  # Print initial pose

def odo_update(ticks_left, ticks_right):  # Define odometry update function
    global x, y, theta  # Use global pose variables
    dl = ticks_left * cm_per_tick  # Convert left ticks to distance
    dr = ticks_right * cm_per_tick  # Convert right ticks to distance
    dc = (dl + dr) / 2.0  # Compute center distance moved
    dtheta = (dr - dl) / wheel_base_cm  # Compute change in heading
    theta += dtheta  # Update heading
    x += dc * math.cos(theta)  # Update x using forward projection
    y += dc * math.sin(theta)  # Update y using forward projection
    print("POSE:UPDATE", round(x, 2), round(y, 2), round(theta, 3))  # Print pose

# Accelerometer (ADXL345) setup
ADXL_ADDR = 0x53  # Set I2C address
REG_POWER_CTL = 0x2D  # Set power control register
REG_DATA_FORMAT = 0x31  # Set data format register
REG_DATAX0 = 0x32  # Set axis data start register
i2c = machine.SoftI2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=100000)  # Create I2C bus
i2c.writeto_mem(ADXL_ADDR, REG_POWER_CTL, b'\x08')  # Enable measurement
i2c.writeto_mem(ADXL_ADDR, REG_DATA_FORMAT, b'\x00')  # Set ±2g range

def read_xyz():  # Define function to read accelerometer axes
    buf = i2c.readfrom_mem(ADXL_ADDR, REG_DATAX0, 6)  # Read 6 bytes for X,Y,Z
    x_a = int.from_bytes(buf[0:2], 'little', signed=True)  # Convert X to signed int
    y_a = int.from_bytes(buf[2:4], 'little', signed=True)  # Convert Y to signed int
    z_a = int.from_bytes(buf[4:6], 'little', signed=True)  # Convert Z to signed int
    return x_a, y_a, z_a  # Return X,Y,Z

corr_gain = 0.0008  # Define heading correction gain
print("CORR:GAIN", corr_gain)  # Print gain

def apply_drift_correction():  # Define function to apply heading correction
    global theta  # Use global heading
    x_a, y_a, z_a = read_xyz()  # Read accelerometer axes
    theta += y_a * corr_gain  # Apply small correction from Y tilt
    # Optional clamp to avoid big jumps
    if theta > math.pi:  # If theta exceeds +pi
        theta -= 2 * math.pi  # Wrap angle
    if theta < -math.pi:  # If theta below -pi
        theta += 2 * math.pi  # Wrap angle
    print("CORR:APPLY", y_a, round(theta, 3))  # Print tilt and new theta

# Optional ultrasonic for obstacle marking
trig = machine.Pin(27, machine.Pin.OUT)  # Create TRIG pin
echo = machine.Pin(25, machine.Pin.IN)  # Create ECHO pin
print("ULTRA:PINS 27/25")  # Print ultrasonic pins

def ultra_cm():  # Define ultrasonic measure function
    trig.value(0)  # Ensure trigger LOW
    time.sleep_us(3)  # Short settle
    trig.value(1)  # Trigger HIGH
    time.sleep_us(10)  # 10 microsecond pulse
    trig.value(0)  # Trigger LOW
    t0 = time.ticks_us()  # Start timeout
    while echo.value() == 0:  # Wait for echo rising
        if time.ticks_diff(time.ticks_us(), t0) > 20000:  # 20 ms timeout
            return 999  # Return large if no echo
    start = time.ticks_us()  # Pulse start time
    while echo.value() == 1:  # Wait while echo HIGH
        pass  # Busy-wait
    end = time.ticks_us()  # Pulse end time
    dur = time.ticks_diff(end, start)  # Pulse duration
    cm = dur // 58  # Convert to cm
    return cm  # Return centimeters

# Mapping storage
visited = set()  # Initialize visited cell set
blocked = set()  # Initialize blocked cell set
grid_cm = 5  # Define grid cell size (cm)
print("MAP:GRID", grid_cm)  # Print grid size

def cell_from_pose():  # Define helper to convert pose to a grid cell
    cx = int(round(x / grid_cm))  # Compute grid x index
    cy = int(round(y / grid_cm))  # Compute grid y index
    return (cx, cy)  # Return cell tuple

def mark_map():  # Define function to mark map cells
    cell = cell_from_pose()  # Get current cell
    visited.add(cell)  # Add cell to visited
    cm = ultra_cm()  # Measure distance
    if cm <= 20:  # If obstacle nearby
        ahead = (cell[0] + 1, cell[1])  # Assume heading approx +X, mark ahead cell
        blocked.add(ahead)  # Add ahead cell to blocked
    print("MAP:VISITED", list(visited))  # Print visited set
    print("MAP:BLOCKED", list(blocked))  # Print blocked set

# Navigation helpers
def angle_wrap(a):  # Define angle wrapping to [-pi, pi]
    while a > math.pi:  # If angle too large
        a -= 2 * math.pi  # Subtract full circle
    while a < -math.pi:  # If angle too small
        a += 2 * math.pi  # Add full circle
    return a  # Return wrapped angle

tx, ty = 30.0, 20.0  # Define target coordinates (cm)
print("NAV:TARGET", tx, ty)  # Print target coordinates

def nav_step():  # Define one navigation step toward target
    global theta  # Use global heading
    dx = tx - x  # Compute delta x to target
    dy = ty - y  # Compute delta y to target
    dist = math.hypot(dx, dy)  # Compute distance
    tgt_ang = math.atan2(dy, dx)  # Compute angle to target
    err = angle_wrap(tgt_ang - theta)  # Compute bearing error
    print("NAV:ERR", round(err, 3), "DIST", round(dist, 2))  # Print error and distance
    if abs(err) > 0.25:  # If heading error large
        odo_update(2, 0)  # Apply small rotation step
    elif dist > 2.0:  # If far from target
        odo_update(3, 3)  # Move forward step
    else:  # If close
        print("NAV:ARRIVED")  # Print arrival

# Trajectory correction
def cross_track_error(px, py, ax, ay, bx, by):  # Define cross-track error function
    vx = bx - ax  # Line vector x
    vy = by - ay  # Line vector y
    wx = px - ax  # Point vector x
    wy = py - ay  # Point vector y
    line_len = math.hypot(vx, vy)  # Line length
    if line_len == 0:  # If zero length
        return 0.0  # Return 0
    sign = 1.0 if (vx * wy - vy * wx) > 0 else -1.0  # Determine side of line
    err = sign * abs(vx * wy - vy * wx) / line_len  # Compute signed distance
    return err  # Return error

def correction_step():  # Define trajectory correction step
    err = cross_track_error(x, y, 0.0, 0.0, tx, ty)  # Compute error to path
    print("CORR:CTE", round(err, 2))  # Print cross-track error
    if abs(err) > 2.0:  # If deviation large
        if err > 0:  # If deviated to left
            odo_update(2, 3)  # Nudge right
        else:  # If deviated to right
            odo_update(3, 2)  # Nudge left
    else:  # If deviation small
        odo_update(3, 3)  # Move forward

print("RUN:IPS")  # Announce main project start
for i in range(12):  # Iterate control steps
    apply_drift_correction()  # Apply heading correction from accelerometer
    nav_step()  # Move toward target using bearing
    correction_step()  # Nudge back toward planned path
    mark_map()  # Mark visited and blocked cells
    time.sleep(0.1)  # Short delay to keep output readable

External explanation

• What it teaches: You built an internal positioning pipeline: wheel‑based odometry, gentle drift correction from accelerometer tilt, a tiny grid map, navigation toward a target, and trajectory nudging.
• Why it works: Odometry updates pose; accelerometer fights drift; a grid captures memory; bearing error points you to the target; cross‑track error keeps you near the intended line.
• Key concept: “Estimate → correct → remember → steer → refine.”

Story time

Your robot doesn’t just move—it knows where it is. It inches forward, checks its balance, remembers the cells it’s visited, and steers back to the path when it wanders. Quiet confidence.

Debugging (2)

Debugging 6.7.1 – Cumulative error in position

Problem: After several moves, pose seems far from reality.
Clues: cm_per_tick too high; wheel_base_cm off; corrections are too strong.
Broken code:

cm_per_tick = 1.2  # Overestimated distance per tick
corr_gain = 0.005  # Correction too strong

Fixed code:

cm_per_tick = 0.8  # Calibrate with a measured 1 m test
corr_gain = 0.0008  # Gentle correction to avoid oscillation
print("DEBUG:CONST", cm_per_tick, corr_gain)  # Print tuned constants

Why it works: Accurate geometry reduces drift; small corrections prevent oscillations.
Avoid next time: Calibrate on a straight tape path and record honest distance.

Debugging 6.7.2 – Overcorrection

Problem: Heading swings back and forth, never settles.
Clues: CORR:APPLY jumps; NAV:ERR flips sign quickly.
Broken code:

theta += y_a * 0.003  # Aggressive tilt-to-heading mapping

Fixed code:

theta += max(-0.01, min(0.01, y_a * corr_gain))  # Clamp correction per step
print("DEBUG:CLAMPED_CORR")  # Show that clamp is active

Why it works: Clamping keeps corrections inside a safe range so heading stabilizes.
Avoid next time: Add averaging and clamps before applying any sensor-based bias.

Final checklist

• Odometry updates print clean POSE values after each move
• Drift correction applies gentle, clamped heading adjustments
• Grid map stores visited/blocked cells with short, labeled prints
• Navigation steps reduce distance to the target and print arrival
• Trajectory correction lowers cross‑track error over several steps

Extras

• 🧠 Student tip: Run a 1 m straight test and count ticks to set cm_per_tick; repeat for wheel_base_cm with a 360° turn.
• 🧑‍🏫 Instructor tip: Have students plot POSE over time in a spreadsheet—seeing drift helps explain calibration.
• 📖 Glossary:
  • Odometry: Estimating position from wheel motion and heading.
  • Cross‑track error: Perpendicular distance from the robot to the intended path line.
  • Heading drift: Slow change in heading due to imperfect mechanics or sensors.
• 💡 Mini tips:
  • Keep battery level consistent during calibration; speed changes affect tick mapping.
  • Use integer grid cells (5 cm) for easy classroom visualization.
  • Label all prints consistently so students can parse logs quickly.