Advanced 3: Control Theory
Prerequisites: Lab 3 (Motor Control)
Overview
In Lab 3, you implemented P-control for line following. This module explores the full theory of feedback control: PID, tuning methods, stability, and system modeling.
You'll learn: - Why P-control alone has limitations - How I and D terms improve performance - Systematic tuning methods - When control systems fail (and how to prevent it)
Part 1: The Feedback Control Loop
Block Diagram
Every feedback controller has this structure:
Setpoint Error Control Process Output
(desired) ──►(+)──────►│Controller│──►│ System │──────┬──►
│-│ └──────────┘ └────────┘ │
▲ │
│ Feedback │
└─────────────│Sensor│◄──────────────────┘
└──────┘
Components: - Setpoint (r): What you want (e.g., line position = 0) - Error (e): Difference between setpoint and actual: \(e = r - y\) - Controller: Calculates control output from error - Process/Plant: The physical system (motors, robot) - Output (y): What you actually get (measured position) - Sensor: Measures output (line sensors)
Mathematical Form
Where: - \(u(t)\) = control signal (motor speed) - \(e(t)\) = error signal (position deviation) - \(f\) = controller function (P, PI, PD, or PID)
Part 2: P, I, and D Explained
Proportional (P) - React to Present
Output is proportional to current error.
Intuition: "The further off target, the harder I push."
Error: ────────╲ ╱────────
╲ ╱
╲__________╱
P Output: ────────╲ ╱────────
╲ ╱
╲__________╱
(same shape as error)
Problem: P-only control often has steady-state error - it settles at a point where output balances disturbance, but not at zero error.
Integral (I) - Remember the Past
Output accumulates based on past errors.
Intuition: "If we've been off target for a while, push harder."
Error: ──────────────────────────
(small constant error)
I Output: ╱
╱
╱
╱
(keeps growing until error is zero!)
Benefit: Eliminates steady-state error Problem: Can cause overshoot and oscillation ("integral windup")
Derivative (D) - Predict the Future
Output responds to rate of change of error.
Intuition: "If error is changing fast, start correcting early."
Error: ──────╲
╲
╲
╲──────────
D Output: ────╲
╲_____
╲
╲________
(spikes on change, zero when steady)
Benefit: Dampens oscillation, speeds response Problem: Amplifies noise (derivative of noisy signal is noisier!)
Part 3: PID Controller
Full PID Equation
Or in discrete form (what you implement):
Implementation
class PIDController:
def __init__(self, kp, ki, kd, dt):
self.kp = kp
self.ki = ki
self.kd = kd
self.dt = dt # Time step in seconds
self.integral = 0
self.prev_error = 0
def update(self, error):
# Proportional term
p_term = self.kp * error
# Integral term
self.integral += error * self.dt
i_term = self.ki * self.integral
# Derivative term
derivative = (error - self.prev_error) / self.dt
d_term = self.kd * derivative
self.prev_error = error
return p_term + i_term + d_term
def reset(self):
self.integral = 0
self.prev_error = 0
When to Use Each Term
| Situation | Use |
|---|---|
| Simple tracking | P only |
| Eliminate steady-state error | P + I |
| Fast response, no overshoot | P + D |
| General purpose | Full PID |
| Noisy sensor | Avoid D (or filter first) |
Part 4: Tuning Methods
Manual Tuning Procedure
- Start with P only (Ki=0, Kd=0)
- Increase Kp until system oscillates
- Reduce Kp by ~30-50%
- Add Ki slowly until steady-state error is eliminated
- Add Kd if overshoot is a problem
Ziegler-Nichols Method
A classic systematic approach:
Step 1: Set Ki=0, Kd=0 Step 2: Increase Kp until sustained oscillation (\(K_u\) = "ultimate gain") Step 3: Measure oscillation period (\(T_u\)) Step 4: Use these formulas:
| Controller | Kp | Ki | Kd |
|---|---|---|---|
| P | 0.5×Ku | 0 | 0 |
| PI | 0.45×Ku | 1.2×Kp/Tu | 0 |
| PID | 0.6×Ku | 2×Kp/Tu | Kp×Tu/8 |
Experiment: Find Ultimate Gain
from picobot import Motors
from machine import Pin
import time
sensors = [Pin(p, Pin.IN) for p in [2, 3, 4, 5]]
motors = Motors()
def get_line_position():
on_line = [s.value() == 0 for s in sensors]
count = sum(on_line)
if count == 0:
return None
weights = [-1.5, -0.5, 0.5, 1.5]
return sum(w for w, v in zip(weights, on_line) if v) / count
# Start with low Kp, increase until oscillation
KP = 50 # Adjust this!
BASE_SPEED = 100
last_time = time.ticks_ms()
positions = []
try:
while True:
now = time.ticks_ms()
pos = get_line_position()
if pos is not None:
# Record position for analysis
positions.append((now, pos))
# P-only control
correction = KP * pos
left = int(BASE_SPEED - correction)
right = int(BASE_SPEED + correction)
motors.set_speeds(left, right)
else:
motors.stop()
time.sleep_ms(10)
except KeyboardInterrupt:
motors.stop()
# Analyze oscillation
if len(positions) > 20:
# Look for zero crossings
crossings = []
for i in range(1, len(positions)):
if positions[i-1][1] * positions[i][1] < 0:
crossings.append(positions[i][0])
if len(crossings) >= 4:
periods = [crossings[i+1] - crossings[i] for i in range(len(crossings)-1)]
avg_period = sum(periods) / len(periods) * 2 # Full cycle
print(f"Oscillation period Tu = {avg_period} ms")
print(f"Ultimate gain Ku = {KP}")
Software Tuning Tools
For serious tuning, record data and analyze:
# Record step response data
data = []
setpoint = 0 # Centered on line
start = time.ticks_ms()
while time.ticks_diff(time.ticks_ms(), start) < 5000:
pos = get_line_position()
now = time.ticks_diff(time.ticks_ms(), start)
if pos is not None:
error = setpoint - pos
output = pid.update(error)
data.append((now, setpoint, pos, error, output))
# Apply to motors
motors.set_speeds(
int(BASE_SPEED - output),
int(BASE_SPEED + output)
)
time.sleep_ms(10)
motors.stop()
# Output as CSV for PC analysis
print("time_ms,setpoint,position,error,output")
for row in data:
print(",".join(str(x) for x in row))
Part 5: Stability and Failure Modes
What Makes Control Unstable?
Gain too high: - System oscillates with increasing amplitude - Eventually saturates or destroys itself
Phase lag: - Correction arrives too late - Pushes when it should pull
Integral windup: - Integral term grows huge during saturation - Takes forever to unwind
Oscillation Patterns
Underdamped (too much gain):
╱╲ ╱╲ ╱╲
╱ ╲ ╱ ╲ ╱ ╲
──╱ ╲╱ ╲╱ ╲── target
(oscillates around target)
Overdamped (too little gain):
──────── target
╱───────
╱─────
───╱ (slow approach)
Critically damped (just right):
────────── target
╱────
╱───
───╱ (fast, no overshoot)
Preventing Integral Windup
class PIDController:
def __init__(self, kp, ki, kd, dt, integral_limit=100):
# ... other init ...
self.integral_limit = integral_limit
def update(self, error):
# ... P and D terms ...
# Integral with anti-windup
self.integral += error * self.dt
self.integral = max(-self.integral_limit,
min(self.integral_limit, self.integral))
i_term = self.ki * self.integral
# ...
Output Limiting
Always limit your control output to valid range:
def update(self, error):
output = p_term + i_term + d_term
# Limit to motor range
output = max(-255, min(255, output))
return output
Part 6: Real-World Complications
Discrete Sampling Effects
Your controller runs at discrete intervals, not continuously. This affects stability!
Rule of thumb: Sample at least 10× faster than your system's natural frequency.
Sensor Noise and Derivative
The D term amplifies noise:
Clean signal: ───────────────────
Derivative: ___________________
Noisy signal: ─╱╲─╱╲─╱╲─╱╲─╱╲─╱╲─
Derivative: ╱╲╱╲╱╲╱╲╱╲╱╲╱╲╱╲╱╲╱ (HUGE!)
Solution: Filter before differentiating:
class FilteredPID:
def __init__(self, kp, ki, kd, dt, alpha=0.1):
# ... other init ...
self.alpha = alpha # Filter coefficient
self.filtered_error = 0
def update(self, error):
# Low-pass filter on error (for derivative)
self.filtered_error = (self.alpha * error +
(1 - self.alpha) * self.filtered_error)
# Use filtered error for derivative
derivative = (self.filtered_error - self.prev_filtered) / self.dt
d_term = self.kd * derivative
self.prev_filtered = self.filtered_error
# ...
Dead Zone (Motor Dead Band)
Motors don't respond to very low PWM values:
Solution: Add minimum PWM or dead band compensation:
def apply_motor(output):
if abs(output) < 5: # Threshold
return 0
elif output > 0:
return output + 30 # Compensate for dead zone
else:
return output - 30
Part 7: Feed-Forward Control
The Limitation of Feedback
Feedback reacts to error after it happens. For predictable disturbances, we can do better!
Feed-Forward + Feedback
Setpoint ──────────────┬────►│Feed-Forward│──┐
│ │ └────────────┘ │
│ ▼ ▼
└──►(+)──►│Feedback│──►(+)──►│System│──►
│-│ └────────┘ └──────┘
▲ │
└───────────────────────────────┘
Example: Line following with known curve
# If you know the track curves right ahead,
# start turning before the error appears!
def control_with_feedforward(position, upcoming_curve):
# Feedback: correct current error
feedback = KP * position
# Feed-forward: anticipate known disturbance
feedforward = CURVE_GAIN * upcoming_curve
return feedback + feedforward
When to Use Feed-Forward
- Repetitive disturbances (known track shape)
- Measurable disturbances (wind, slope)
- Model-based prediction
Part 8: System Modeling (Conceptual)
Transfer Functions
Engineers model systems mathematically:
For a simple DC motor:
Where: - \(K\) = gain (how much output per unit input) - \(\tau\) = time constant (how fast it responds)
Step Response
Apply a step input, measure the output:
Input: ┌────────────────
│
─────┘
Output: ┌─────────────── (final value)
╱
╱
╱
╱
──────┘
│←─ τ ─→│ (time to reach 63%)
From step response you can estimate: - Gain K = final value / step size - Time constant τ = time to reach 63% of final value
Measuring Your Motor
from picobot import Motors
import time
motors = Motors()
# Step response test
PWM_STEP = 150
DURATION_MS = 2000
SAMPLE_RATE_MS = 10
# Apply step and record (if you have encoders)
# Otherwise, observe visually or use IMU for rotation rate
data = []
motors.set_speeds(0, 0)
time.sleep_ms(500)
start = time.ticks_ms()
motors.set_speeds(PWM_STEP, PWM_STEP)
while time.ticks_diff(time.ticks_ms(), start) < DURATION_MS:
now = time.ticks_diff(time.ticks_ms(), start)
# Record speed measurement here (encoder, IMU, etc.)
# data.append((now, measured_speed))
time.sleep_ms(SAMPLE_RATE_MS)
motors.stop()
# Analyze data to find K and tau
Mini-Project: Full PID Line Follower
Part A: Characterize the System
- Measure motor step response (if encoders available)
- Measure line position response to motor input
- Estimate system gain and time constant
Part B: Implement Full PID
- Start with your Lab 3 P-controller
- Add integral term with anti-windup
- Add filtered derivative term
- Test on line track
Part C: Systematic Tuning
- Apply Ziegler-Nichols method
- Find ultimate gain and period
- Calculate PID parameters
- Fine-tune manually
Part D: Compare Performance
Create a test that measures: - Settling time (how fast it corrects) - Overshoot (how much it overshoots target) - Steady-state error (final offset from target) - Maximum speed without losing line
Compare P-only vs PI vs PD vs PID.
Deliverable
Report including: - System characterization data - PID implementation code - Ziegler-Nichols calculations - Performance comparison table - Video of optimized line follower
Key Takeaways
- P reacts, I remembers, D predicts - each term serves a purpose
- Start simple - P-only is often good enough
- Tune systematically - don't just guess
- Watch for instability - too much gain causes oscillation
- Real systems are messy - dead zones, noise, delays all matter
- Feed-forward helps - anticipate when you can predict
Professional Context: Industrial & Automotive Control
Your PID line follower is a great introduction. Professional systems use sophisticated control architectures for performance, safety, and certification. Here's how they compare:
Control System Comparison
| Feature | Your Robot PID | Industrial PLC | Automotive ECU | Aerospace |
|---|---|---|---|---|
| Controller | Software PID | FB_PID function block | Model-based control | Flight control laws |
| Tuning | Manual trial | Auto-tune, gain scheduling | Calibration tables | Certified, fixed |
| Sample rate | 10-100 Hz | 100-1000 Hz | 1-10 kHz | 100+ Hz |
| Sensors | Optocouplers | Industrial 4-20mA | Automotive-grade | Redundant, voted |
| Actuators | Hobby motors | Servo drives | EPS motor, injectors | Hydraulics, fly-by-wire |
| Safety | None | SIL 2/3 | ASIL-D | DAL-A |
| Certification | None | IEC 61508 | ISO 26262 | DO-178C |
Industrial PID (PLC/DCS)
PLCs and Distributed Control Systems use standardized PID function blocks:
Your Python PID:
output = kp*e + ki*integral + kd*derivative
(You manage anti-windup, limits, manually)
Industrial PID (IEC 61131-3):
┌────────────────────────────────────────────────────┐
│ FB_PID (Standard Function Block) │
│ ┌──────────────────────────────────────────────┐ │
│ │ Inputs: │ Parameters: │ │
│ │ SP (setpoint) │ Kp, Ti, Td (tuning) │ │
│ │ PV (process) │ Cycle time │ │
│ │ AUTO/MAN │ Output limits │ │
│ ├────────────────┼─────────────────────────────┤ │
│ │ Built-in features: │ │
│ │ • Anti-windup (automatic) │ │
│ │ • Bumpless transfer (auto↔manual) │ │
│ │ • Derivative filter │ │
│ │ • Setpoint ramping │ │
│ │ • Output rate limiting │ │
│ │ • Alarm outputs │ │
│ └──────────────────────────────────────────────┘ │
└────────────────────────────────────────────────────┘
Typical Ti, Td notation (not Ki, Kd):
P = Kp × error
I = Kp/Ti × ∫error dt (Ti = integral time)
D = Kp × Td × d(error)/dt (Td = derivative time)
Automotive Control Examples
Modern vehicles have dozens of control loops running simultaneously:
┌─────────────────────────────────────────────────────────────────┐
│ Vehicle Control Systems │
├─────────────────────────────────────────────────────────────────┤
│ │
│ Engine Management (1-10 kHz): │
│ ├── Air-fuel ratio control (lambda/O2 sensor) │
│ ├── Idle speed control (PID + feedforward) │
│ ├── Knock control (adaptive spark retard) │
│ └── Torque control (driver demand → actual torque) │
│ │
│ Transmission Control: │
│ ├── Shift quality control (clutch pressure profiles) │
│ ├── Torque converter lockup (slip control) │
│ └── Gear selection strategy (not just PID!) │
│ │
│ Chassis/Safety (1-10 kHz): │
│ ├── ABS - Anti-lock Braking (wheel slip control) │
│ ├── ESC - Electronic Stability Control (yaw rate) │
│ ├── EPS - Electric Power Steering (torque assist) │
│ └── ACC - Adaptive Cruise Control (distance/speed) │
│ │
│ Electric Drive (10-20 kHz): │
│ ├── Field-Oriented Control (torque/flux control) │
│ ├── Current control loops (very fast, ~20kHz) │
│ └── Speed control (outer loop, ~1kHz) │
│ │
└─────────────────────────────────────────────────────────────────┘
ESC (Electronic Stability Control) - A Real Example
Your line follower tries to stay centered. ESC tries to keep a car stable:
Your line follower:
Setpoint: Line center (position = 0)
Sensor: 4 optocouplers
Control: P or PID on position error
Actuator: Two DC motors (left/right speed)
error = 0 - line_position
correction = Kp * error
left_motor = base_speed - correction
right_motor = base_speed + correction
ESC (Electronic Stability Control):
Setpoint: Desired yaw rate (from steering angle + speed)
Sensors: - Yaw rate gyroscope
- Lateral acceleration
- Wheel speeds (4×)
- Steering angle
Control: Compare desired vs actual yaw rate
Actuator: Individual wheel brakes
desired_yaw = f(steering_angle, speed) // Vehicle model
actual_yaw = yaw_gyroscope
yaw_error = desired_yaw - actual_yaw
if understeer: // Car going straight when should turn
brake_inner_rear() // Rotate car into turn
if oversteer: // Car spinning too much
brake_outer_front() // Stabilize
Plus: Friction estimation, tire model, driver intent detection...
Advanced Control Methods
Beyond PID, professionals use:
┌─────────────────────────────────────────────────────────────────┐
│ Control Method Hierarchy │
├─────────────────────────────────────────────────────────────────┤
│ │
│ PID (Classical) ← Your robot, most industrial │
│ ├── Simple, robust │
│ ├── Works for many systems │
│ └── Tuning can be trial-and-error │
│ │
│ State-Space (Modern) ← Aerospace, advanced automotive │
│ ├── LQR (Linear Quadratic Regulator) │
│ ├── LQG (LQR + Kalman filter) │
│ ├── Pole placement │
│ └── Requires system model │
│ │
│ MPC (Model Predictive) ← Autonomous vehicles, process │
│ ├── Predicts future states │
│ ├── Optimizes over horizon │
│ ├── Handles constraints naturally │
│ └── Computationally expensive │
│ │
│ Adaptive Control ← Varying systems │
│ ├── Gain scheduling (parameter lookup) │
│ ├── MRAC (Model Reference Adaptive) │
│ └── Self-tuning regulators │
│ │
│ Robust Control ← Uncertain systems │
│ ├── H-infinity │
│ ├── mu-synthesis │
│ └── Guaranteed stability with model uncertainty │
│ │
│ Nonlinear Control ← Highly nonlinear systems │
│ ├── Sliding mode │
│ ├── Feedback linearization │
│ └── Lyapunov-based design │
│ │
└─────────────────────────────────────────────────────────────────┘
Where PID is used: ~90% of industrial control loops
Where MPC is used: Process industry, autonomous vehicles
Where LQR is used: Aerospace, multi-variable systems
Cascaded Control
Professional systems often use nested control loops:
Your robot (single loop):
Setpoint ──► PID ──► Motors
▲
└── Sensors
Industrial (cascaded):
┌─────────────────────────────────────────────────────────────┐
│ │
│ Position Setpoint │
│ │ │
│ ▼ │
│ ┌─────────┐ Velocity ┌─────────┐ Torque │
│ │Position │ Setpoint │Velocity │ Setpoint │
│ │ PID │────────────────►│ PID │─────────────┐ │
│ └────▲────┘ └────▲────┘ │ │
│ │ │ │ │
│ │ ┌─────────────────┘ ▼ │
│ │ │ ┌──────────┐│
│ │ │ │ Motor/ ││
│ │ │ │ Drive ││
│ │ │ └────┬─────┘│
│ │ │ │ │
│ Position Velocity Current │
│ Sensor Sensor Sensor │
│ (Encoder) (Encoder │
│ derivative) │
└─────────────────────────────────────────────────────────────┘
Outer loop: Slow (100 Hz), position control
Middle loop: Medium (1 kHz), velocity control
Inner loop: Fast (10+ kHz), current/torque control
Why cascade?
- Inner loops faster → better disturbance rejection
- Each loop has clear purpose
- Easier to tune (start from inner loop)
Gain Scheduling
When a system behaves differently in different conditions:
Your robot:
One set of Kp, Ki, Kd values
Works at one speed, one surface
Gain-scheduled control:
┌─────────────────────────────────────────────────────────────┐
│ │
│ Operating Condition PID Parameters │
│ ───────────────────── ────────────── │
│ Low speed (< 30%) Kp=1.0, Ki=0.5, Kd=0.1 │
│ Medium speed (30-70%) Kp=1.5, Ki=0.3, Kd=0.2 │
│ High speed (> 70%) Kp=2.0, Ki=0.1, Kd=0.3 │
│ │
│ Implementation: │
│ if speed < 0.3: │
│ kp, ki, kd = PARAMS_LOW │
│ elif speed < 0.7: │
│ kp, ki, kd = PARAMS_MED │
│ else: │
│ kp, ki, kd = PARAMS_HIGH │
│ │
└─────────────────────────────────────────────────────────────┘
Automotive example:
EPS (Electric Power Steering) assist varies with speed:
- Low speed (parking): High assist → easy steering
- High speed (highway): Low assist → stable steering
Model Predictive Control (MPC) - Brief Introduction
The most advanced control method in common use:
PID (reactive):
Look at current error → calculate correction
MPC (predictive):
1. Predict future states using model
2. Optimize control over prediction horizon
3. Apply first control action
4. Repeat next timestep
┌─────────────────────────────────────────────────────────────┐
│ │
│ Time: now +1 +2 +3 +4 ... +N │
│ ├──────────┼─────┼─────┼─────┼──────────┤ │
│ Predict: │ current │ ──► │ ──► │ ──► │ ──► ... │ │
│ │ state │ │ │ │ │ │
│ │
│ Optimize: Find u[0], u[1], u[2], ... that minimize: │
│ - Tracking error (follow setpoint) │
│ - Control effort (don't use too much) │
│ - Constraint violations (stay within limits) │
│ │
│ Apply: Only u[0] (first control action) │
│ Repeat: Next timestep, re-optimize from new state │
│ │
└─────────────────────────────────────────────────────────────┘
Used in:
- Autonomous vehicle path planning
- Process industry (chemical plants)
- Building HVAC optimization
- Tesla Autopilot lateral control (reportedly)
Industry Tools
| Tool | Use | Cost |
|---|---|---|
| MATLAB/Simulink | Control design, simulation, code gen | $$$$ (or free academic) |
| LabVIEW | Data acquisition, control systems | $$$ |
| Scilab/Xcos | MATLAB alternative | Free |
| GNU Octave | MATLAB alternative | Free |
| Python Control | Control system analysis | Free |
| PLC Function Blocks | Industrial PID | Included in PLC |
| dSPACE | Rapid prototyping, HIL | $$$$ |
# Python Control library example (free alternative to MATLAB)
# pip install control
import control as ct
import numpy as np
# Define a simple motor transfer function
# G(s) = K / (tau*s + 1)
K = 10 # DC gain
tau = 0.1 # Time constant
motor = ct.tf([K], [tau, 1])
# Design a PID controller
Kp, Ki, Kd = 1.0, 5.0, 0.05
pid = ct.tf([Kd, Kp, Ki], [1, 0])
# Closed-loop system
closed_loop = ct.feedback(pid * motor, 1)
# Step response
t = np.linspace(0, 1, 100)
t_out, y_out = ct.step_response(closed_loop, t)
# Analyze stability, margins, etc.
gm, pm, wcg, wcp = ct.margin(pid * motor)
print(f"Phase margin: {pm:.1f} degrees")
print(f"Gain margin: {20*np.log10(gm):.1f} dB")
What the Industry Uses
| Manufacturer | Product | Application |
|---|---|---|
| Siemens | TIA Portal | PLC control, motion |
| Rockwell | Studio 5000 | Industrial automation |
| MathWorks | Simulink | Control design, code gen |
| dSPACE | SCALEXIO | Automotive HIL testing |
| National Instruments | LabVIEW | Test, control systems |
| Vector | CANape | Automotive calibration |
| ETAS | INCA | ECU calibration |
Hardware Limits Principle
What Software Can and Cannot Fix
Software CAN improve: - Control algorithm → better tuning, advanced methods - Noise rejection → digital filtering, estimation - Adaptability → gain scheduling, parameter adjustment - Diagnostics → fault detection, monitoring - Tuning process → auto-tune algorithms
Software CANNOT fix: - Sensor resolution → 10-bit ADC can't become 16-bit - Sample rate → limited by hardware and bus speed - Actuator bandwidth → motor can't respond faster than physics - Latency → I2C/Python overhead adds milliseconds - Sensor noise floor → physics of the sensor - Plant dynamics → slow system is slow regardless of controller
The lesson: A perfect PID controller on slow hardware with noisy sensors will underperform a basic P controller on fast hardware with good sensors. Before tuning your algorithm, check: Is my sample rate fast enough? Is my sensor noise acceptable? Can my actuator respond quickly enough? These are hardware questions, not software questions.
Real Example: Line Following Performance Limits
| Factor | Your Robot | What Limits Performance |
|---|---|---|
| Sample rate | ~50-100 Hz (Python) | GC pauses, I2C overhead |
| Sensor resolution | 4 discrete points | Can't detect small errors |
| Motor response | ~100ms to change speed | Inertia, PWM frequency |
| Algorithm | P or PID | Already good enough! |
Bottleneck analysis:
Q: Why does my robot oscillate at high speed?
A: Check hardware first!
1. Sample rate: 50 Hz = 20ms between updates
At 1 m/s, robot moves 20mm between corrections
→ Try faster loop (C, not Python)
2. Sensor latency: 4 optocouplers via ADC
Each read takes time, total ~5ms
→ Use direct GPIO reads
3. Motor response: PWM to speed change
Motor + gearbox inertia limits response
→ Higher PWM frequency, better motor
4. Algorithm: PID tuning
Only tune AFTER hardware is optimized
→ Kp too high causes oscillation, but
hardware limits set maximum useful Kp
Further Reading
- PID Without a PhD - Tim Wescott's classic article
- Control System Design - Åström & Murray textbook chapter
- Motion Control Primer - Practical motion control guide
- Python Control Library - Free control system analysis
- MPC Tutorial - Model Predictive Control introduction
← Back: Real-Time Concepts | Advanced Track Overview | Next: Sensor Fusion →