# Task: Build Scanbot Speed Controller
**Priority:** HIGH
**Target file:** `Scanbot/src/control/speed_controller.py`
**Dependencies:** `Scanbot/src/control/pid_controller.py` (build that first), `Scanbot/src/control/motor_controller.py` (existing)
## What to Build
A closed-loop speed controller that uses PID + EZKontrol RPM feedback to hold a target ground speed. Currently Scanbot uses open-loop throttle% → RPM with no feedback.
## Vehicle Constants (from Scanbot/config/vehicle.json)
```
gear_ratio = 27.8 (motor sprocket 7T / drive sprocket 50T * diff 3.889)
tire_circumference = 1.995m (25" tire)
rpm_to_mps = tire_circumference / (gear_ratio * 60) = 1.995 / (27.8 * 60) = 0.001196
```
So: `ground_speed_mps = motor_rpm * 0.001196`
And: `target_rpm = target_speed_mps / 0.001196`
## Class Specification
```python
import logging
import json
from pathlib import Path
from typing import Optional
from src.control.pid_controller import PIDController
from src.control.motor_controller import MotorController
logger = logging.getLogger(__name__)
class SpeedController:
"""Closed-loop speed control using drive motor RPM feedback and PID."""
def __init__(self, motor: MotorController, vehicle_config_path: str = "config/vehicle.json"):
self.motor = motor
self._target_speed_mps = 0.0
self._actual_speed_mps = 0.0
# Load vehicle geometry
config = self._load_vehicle_config(vehicle_config_path)
self.gear_ratio = config.get("total_gear_ratio", 27.8)
self.tire_circumference_m = config.get("tire_circumference_m", 1.995)
self.rpm_to_mps = self.tire_circumference_m / (self.gear_ratio * 60.0)
# PID controller for speed
# Output is throttle percentage adjustment (0-100)
self.pid = PIDController(
kp=0.5, ki=0.1, kd=0.0,
output_min=0.0, output_max=100.0,
deadband=0.05, # 0.05 m/s deadband
)
# Feedforward table: throttle% → expected RPM (populated by calibration)
self.throttle_to_rpm_table: list[tuple[float, float]] = []
def _load_vehicle_config(self, path: str) -> dict:
"""Load vehicle configuration from JSON."""
try:
with open(path, 'r') as f:
return json.load(f)
except (FileNotFoundError, json.JSONDecodeError) as e:
logger.warning(f"Could not load vehicle config: {e}, using defaults")
return {}
def set_target_speed(self, speed_mps: float):
"""Set target ground speed in m/s."""
self._target_speed_mps = max(0.0, speed_mps)
def update(self, dt: float) -> float:
"""Update speed controller. Call at control loop rate (20 Hz).
Reads actual RPM from motor controller, computes PID output,
applies throttle command.
Returns actual ground speed in m/s.
"""
# 1. Read actual RPM from motor
actual_rpm = self.motor.speed_rpm # From CAN feedback
self._actual_speed_mps = actual_rpm * self.rpm_to_mps
# 2. If target is zero, just set throttle to 0
if self._target_speed_mps < 0.01:
self.motor.set_throttle(0.0)
self.pid.reset()
return self._actual_speed_mps
# 3. Compute feedforward (if calibration table exists)
ff_throttle = self._feedforward(self._target_speed_mps)
# 4. PID correction
pid_output = self.pid.compute(
self._target_speed_mps, self._actual_speed_mps, dt
)
# 5. Combine feedforward + PID
total_throttle = ff_throttle + pid_output
total_throttle = max(0.0, min(100.0, total_throttle))
# 6. Apply to motor
self.motor.set_throttle(total_throttle)
return self._actual_speed_mps
def _feedforward(self, target_speed_mps: float) -> float:
"""Lookup feedforward throttle from calibration table.
If no table exists, use simple linear estimate.
"""
if not self.throttle_to_rpm_table:
# Linear estimate: assume max_speed at 100% throttle
max_speed = 3000 * self.rpm_to_mps # 3000 RPM max
return (target_speed_mps / max_speed) * 100.0 if max_speed > 0 else 0.0
# Convert target speed to target RPM
target_rpm = target_speed_mps / self.rpm_to_mps
# Linear interpolation in table
# Table: [(throttle%, rpm), ...] sorted by throttle
for i in range(len(self.throttle_to_rpm_table) - 1):
t1, r1 = self.throttle_to_rpm_table[i]
t2, r2 = self.throttle_to_rpm_table[i + 1]
if r1 <= target_rpm <= r2:
frac = (target_rpm - r1) / (r2 - r1) if r2 != r1 else 0
return t1 + frac * (t2 - t1)
# Beyond table range
if target_rpm <= self.throttle_to_rpm_table[0][1]:
return self.throttle_to_rpm_table[0][0]
return self.throttle_to_rpm_table[-1][0]
def set_calibration_table(self, table: list[tuple[float, float]]):
"""Set feedforward calibration table from auto-tune.
Args:
table: List of (throttle_percent, measured_rpm) pairs, sorted by throttle.
"""
self.throttle_to_rpm_table = sorted(table, key=lambda x: x[0])
logger.info(f"Speed controller calibration table set: {len(table)} points")
def set_pid_gains(self, kp: float, ki: float, kd: float):
"""Update PID gains (from auto-tune)."""
self.pid.set_gains(kp=kp, ki=ki, kd=kd)
@property
def actual_speed_mps(self) -> float:
return self._actual_speed_mps
@property
def target_speed_mps(self) -> float:
return self._target_speed_mps
@property
def speed_error_mps(self) -> float:
return self._target_speed_mps - self._actual_speed_mps
def get_state(self) -> dict:
"""Get controller state for telemetry."""
return {
"target_speed_mps": round(self._target_speed_mps, 3),
"actual_speed_mps": round(self._actual_speed_mps, 3),
"speed_error_mps": round(self.speed_error_mps, 3),
"pid": self.pid.get_state(),
"has_calibration": len(self.throttle_to_rpm_table) > 0,
}
```
## Key Requirements
1. **Feedforward + PID**: Use calibration table for approximate throttle, PID corrects the error
2. **Graceful fallback**: If no calibration table, use linear estimate
3. **Zero speed**: When target is 0, set throttle to 0 directly (don't PID to zero)
4. **Read `motor.speed_rpm`**: This is the actual RPM from EZKontrol CAN feedback
5. **Vehicle config from JSON**: Load gear_ratio and tire_circumference from `config/vehicle.json`