Advanced 15: Drive to Distance

Prerequisites: Advanced 14 (Drive Straight) Time: ~60 min

---

Overview

You can now drive straight — but "drive for 3 seconds" is useless when you need to stop at a precise point. This module uses encoder odometry to command exact distances: "drive 500 mm, then stop." You'll discover why naive stopping overshoots, build a trapezoidal velocity profile (ramp up → cruise → ramp down), and prove it works by driving a square.

You'll learn:

• Encoder-based odometry: integrating ticks into distance
• Why abrupt stopping overshoots (momentum + control latency)
• Trapezoidal velocity profiles for smooth, accurate motion
• Position-based stopping with braking strategy
• Verification: driving a closed square and measuring closure error

---

Part 1: Naive Distance Driving (~15 min)

Task 1 — Drive and Stop at Distance

The simplest approach: drive at a target speed until the encoder says you've gone far enough:

from picobot import Robot
import time

robot = Robot(encoders=True)

# === YOUR CALIBRATION ===
MM_PER_TICK = ___  # From Module 13
# ========================

TARGET_MM = 500
TARGET_SPEED = 400  # ticks/sec
KP = ___  # Your tuned value from Module 14
pwm_left = 0.0
pwm_right = 0.0

print(f"Drive exactly {TARGET_MM} mm")
input("Place robot at starting mark. Press Enter...")

robot.encoders.reset()

try:
    while True:
        robot.encoders.update()
        avg_ticks = (robot.encoders.left.ticks + robot.encoders.right.ticks) / 2
        distance = avg_ticks * MM_PER_TICK

        if distance >= TARGET_MM:
            break

        error_l = TARGET_SPEED - robot.encoders.left.speed_tps
        pwm_left = max(0, min(255, pwm_left + KP * error_l))
        error_r = TARGET_SPEED - robot.encoders.right.speed_tps
        pwm_right = max(0, min(255, pwm_right + KP * error_r))
        robot.set_motors(int(pwm_left), int(pwm_right))
        time.sleep(0.05)
finally:
    robot.stop()

# Let robot coast to a stop, then measure final distance
time.sleep(0.3)
final_ticks = (robot.encoders.left.ticks + robot.encoders.right.ticks) / 2
final_mm = final_ticks * MM_PER_TICK
overshoot = final_mm - TARGET_MM

print(f"Target:    {TARGET_MM} mm")
print(f"Actual:    {final_mm:.0f} mm")
print(f"Overshoot: {overshoot:.0f} mm")

Run 3 times at different speeds:

Target speed	Target distance	Actual distance	Overshoot
300 tps	500 mm
500 tps	500 mm
700 tps	500 mm

Observation: Higher speed → more overshoot. The robot has momentum — it can't stop instantly when you cut power. The wheels keep turning after robot.stop().

---

Part 2: Trapezoidal Velocity Profile (~25 min)

The Solution: Slow Down Before Stopping

Instead of running at full speed until the last moment, plan the speed profile in advance:

Speed
  ▲
  │      ┌────────────────┐
  │     /│    CRUISE       │\
  │    / │                 │ \
  │   /  │                 │  \
  │  /   │                 │   \
  │ /    │                 │    \
  └──────┴─────────────────┴──────► Distance
   RAMP UP                  RAMP DOWN

Three phases:

1. Ramp up — Gradually increase speed from zero to cruise speed
2. Cruise — Hold constant speed for the middle portion
3. Ramp down — Gradually decrease speed as you approach the target

This is called a trapezoidal profile because the speed-vs-time graph looks like a trapezoid.

Task 2 — Implement Trapezoidal Profile

from picobot import Robot
import time

robot = Robot(encoders=True)

# === YOUR CALIBRATION ===
MM_PER_TICK = ___
# ========================

TARGET_MM = 500
CRUISE_SPEED = 500    # ticks/sec
RAMP_DISTANCE = 100   # mm to spend ramping up/down
KP = ___

pwm_left = 0.0
pwm_right = 0.0

print(f"Trapezoidal drive: {TARGET_MM} mm")
input("Press Enter...")

robot.encoders.reset()

try:
    while True:
        robot.encoders.update()
        avg_ticks = (robot.encoders.left.ticks + robot.encoders.right.ticks) / 2
        distance = avg_ticks * MM_PER_TICK
        remaining = TARGET_MM - distance

        if remaining <= 0:
            break

        # Compute target speed based on position
        if distance < RAMP_DISTANCE:
            # Ramp up: speed proportional to distance traveled
            target = CRUISE_SPEED * (distance / RAMP_DISTANCE)
            target = max(target, CRUISE_SPEED * 0.2)  # Minimum to overcome dead zone
        elif remaining < RAMP_DISTANCE:
            # Ramp down: speed proportional to remaining distance
            target = CRUISE_SPEED * (remaining / RAMP_DISTANCE)
            target = max(target, CRUISE_SPEED * 0.15)  # Don't stall
        else:
            # Cruise
            target = CRUISE_SPEED

        # Dual speed controllers
        error_l = target - robot.encoders.left.speed_tps
        pwm_left = max(0, min(255, pwm_left + KP * error_l))
        error_r = target - robot.encoders.right.speed_tps
        pwm_right = max(0, min(255, pwm_right + KP * error_r))
        robot.set_motors(int(pwm_left), int(pwm_right))

        time.sleep(0.05)
finally:
    robot.stop()

time.sleep(0.3)
final_ticks = (robot.encoders.left.ticks + robot.encoders.right.ticks) / 2
final_mm = final_ticks * MM_PER_TICK
print(f"Target:    {TARGET_MM} mm")
print(f"Actual:    {final_mm:.0f} mm")
print(f"Error:     {final_mm - TARGET_MM:+.0f} mm")

Compare naive vs trapezoidal:

Method	Target	Actual	Error
Naive (full speed)	500 mm
Trapezoidal	500 mm

Task 3 — Tune the Profile

Experiment with different ramp distances:

Ramp distance	Cruise speed	Final error	Motion quality
50 mm	500 tps
100 mm	500 tps
150 mm	500 tps
100 mm	300 tps
100 mm	700 tps

Tuning Guidance

• Short ramps: Faster overall but may still overshoot at high cruise speed
• Long ramps: More accurate stopping but slower overall travel time
• Rule of thumb: Ramp distance should be at least 15-20% of total distance

---

Part 3: The Square Challenge (~15 min)

Task 4 — Drive a Square

Drive 4 sides of 300 mm each. For now, use time-based turns (you'll get encoder-based turns in Module 17). The point is to test distance accuracy:

from picobot import Robot
import time

robot = Robot(encoders=True)

MM_PER_TICK = ___
SIDE_LENGTH = 300     # mm
CRUISE_SPEED = 400
RAMP_DISTANCE = 80
KP = ___
TURN_TIME = 0.6       # Adjust for ~90° turn
TURN_PWM = 70

def drive_distance(mm):
    """Drive straight for a given distance using trapezoidal profile."""
    pwm_l, pwm_r = 0.0, 0.0
    robot.encoders.reset()

    while True:
        robot.encoders.update()
        avg = (robot.encoders.left.ticks + robot.encoders.right.ticks) / 2
        dist = avg * MM_PER_TICK
        remaining = mm - dist

        if remaining <= 0:
            break

        if dist < RAMP_DISTANCE:
            target = max(CRUISE_SPEED * (dist / RAMP_DISTANCE), CRUISE_SPEED * 0.2)
        elif remaining < RAMP_DISTANCE:
            target = max(CRUISE_SPEED * (remaining / RAMP_DISTANCE), CRUISE_SPEED * 0.15)
        else:
            target = CRUISE_SPEED

        err_l = target - robot.encoders.left.speed_tps
        pwm_l = max(0, min(255, pwm_l + KP * err_l))
        err_r = target - robot.encoders.right.speed_tps
        pwm_r = max(0, min(255, pwm_r + KP * err_r))
        robot.set_motors(int(pwm_l), int(pwm_r))
        time.sleep(0.05)

    robot.stop()
    time.sleep(0.2)

def turn_right():
    """Simple time-based right turn. Refine in Module 17."""
    robot.set_motors(TURN_PWM, -TURN_PWM)
    time.sleep(TURN_TIME)
    robot.stop()
    time.sleep(0.2)

# --- Drive the square ---
print("Driving a 300 mm square")
input("Mark starting position. Press Enter...")

for side in range(4):
    print(f"Side {side + 1}...")
    drive_distance(SIDE_LENGTH)
    if side < 3:
        turn_right()

print("Done! Measure how far the robot is from the starting mark.")

Measure closure error — the distance between the final position and the starting mark: ______ mm

Note

Most of the closure error will come from the turns (still time-based, not encoder-based). The straight segments should be accurate to within a few mm. Module 17 will fix the turns.

Checkpoint — Precise Distance Driving

You should be able to drive distances within ±5 mm of the target. The trapezoidal profile should produce noticeably smoother starts and stops than the naive approach. If your accuracy is worse, revisit your calibration values from Module 13.

---

What You Discovered

Concept	What You Learned
Odometry	Integrating ticks over time to measure distance traveled
Overshoot	Momentum means you can't stop instantly — plan for it
Trapezoidal profile	Ramp up → cruise → ramp down for smooth, accurate motion
Closure error	A square that returns to start tests cumulative accuracy

---

What's Next?

You can drive precise distances, but how well is your controller actually performing? Next: Speed Control Lab — log your controller data to CSV, plot it, and systematically tune Kp using real data.

---

← Back to Advanced Topics