Welcome back to the command center! Over the past eight sessions, we have assembled a robot, wired its nervous system, and taught it how to sense its own motion by calculating RPM. We have all the individual pieces of a sophisticated control system laid out before us.
Today is the day we connect them all.
We will take the theory of the PID controller from Session 3 and forge it into real, working Arduino code. We will write the program that takes the error—the difference between our desired speed and the actual speed—and uses it to compute the perfect correction. This is the moment we finally close the loop and give our robot’s brain the power to control its body with intelligence and precision.
Let’s write the code that makes our robot smart.
The Grand Unification: Combining Our Code
Our starting point is the RPM calculator code from Session 8. It already contains the two most important parts of our feedback loop: the encoder-reading interrupt that measures the wheel’s position and the logic that calculates the actual_rpm.
Now, we will add the “brain”—the PID algorithm—to compare, compute, and correct.
The core logic of our PID controller will live inside our main loop. At a regular interval, it will:
- Calculate the current RPM (our measurement).
- Compare this to our target RPM to find the error.
- Compute the Proportional, Integral, and Derivative terms based on this error.
- Sum these terms to get a final control output.
- Send this output to the motor driver as a new power level.
This cycle, running many times per second, will allow the robot to constantly adjust its own speed to match our goal, effectively fighting against disturbances like friction or changes in battery voltage. 1
The Full PID Sketch
Here is the complete Arduino sketch for a single-wheel PID speed controller. It combines everything we’ve learned so far. Copy this into your Arduino IDE. As before, you’ll need to update the hardware constants to match your specific motors.
C++
// --- Pin Definitions ---
// Motor Driver Pins (Front-Left)
#define ENA 2
#define IN1 22
#define IN2 24
// Encoder Pins (Front-Left)
#define ENCODER_A 21
#define ENCODER_B 20
// --- Hardware Constants ---
// CHANGE THESE VALUES TO MATCH YOUR MOTOR AND GEARBOX
const float TICKS_PER_MOTOR_REV = 28.0;
const float GEAR_RATIO = 30.21;
const float TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * GEAR_RATIO;
// --- PID Constants ---
// These are the "tuning knobs" for our controller.
// We'll learn how to tune these in the next session.
float Kp = 5.0; // Proportional gain
float Ki = 0.5; // Integral gain
float Kd = 0.1; // Derivative gain
// --- Global Variables ---
// For Encoder
volatile long encoder_ticks = 0;
// For RPM Calculation
long previous_ticks = 0;
unsigned long previous_time_us = 0;
float actual_rpm = 0.0;
// For PID Controller
float target_rpm = 100.0; // Our desired speed
float error = 0;
float integral_error = 0;
float previous_error = 0;
unsigned long previous_pid_time_us = 0;
// Interrupt Service Routine (ISR) - runs every time the encoder ticks
void readEncoder() {
int b = digitalRead(ENCODER_B);
if (b > 0) {
encoder_ticks++;
} else {
encoder_ticks--;
}
}
void setup() {
Serial.begin(115200);
// Motor Driver Pin Setup
pinMode(ENA, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
// Encoder Pin Setup
pinMode(ENCODER_A, INPUT_PULLUP);
pinMode(ENCODER_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCODER_A), readEncoder, CHANGE);
// Initialize timers
previous_time_us = micros();
previous_pid_time_us = micros();
}
void loop() {
// This loop runs as fast as possible.
// We will calculate RPM and PID at a fixed interval (e.g., every 20ms)
unsigned long current_time_us = micros();
if (current_time_us - previous_pid_time_us >= 20000) { // 20ms interval
// --- 1. Calculate Actual RPM ---
long current_ticks = encoder_ticks;
float delta_time_s = (current_time_us - previous_time_us) / 1000000.0;
float ticks_per_second = (current_ticks - previous_ticks) / delta_time_s;
actual_rpm = (ticks_per_second / TICKS_PER_WHEEL_REV) * 60.0;
previous_ticks = current_ticks;
previous_time_us = current_time_us;
// --- 2. PID Calculation ---
// Calculate the error
error = target_rpm - actual_rpm;
// Calculate the integral term (with anti-windup)
integral_error += error * delta_time_s;
// Calculate the derivative term
float derivative_error = (error - previous_error) / delta_time_s;
// Sum the P, I, and D terms to get the output
float output = (Kp * error) + (Ki * integral_error) + (Kd * derivative_error);
// Update the previous error for the next iteration
previous_error = error;
// --- 3. Apply Control Signal to Motor ---
// Constrain the output to the valid PWM range (0-255)
int motor_power = constrain(output, 0, 255);
// Set motor direction (assuming target_rpm is always positive for now)
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
// Write the power to the motor
analogWrite(ENA, motor_power);
// --- 4. Debugging Output ---
Serial.print("Target RPM: ");
Serial.print(target_rpm);
Serial.print(" | Actual RPM: ");
Serial.print(actual_rpm);
Serial.print(" | Output: ");
Serial.println(motor_power);
// Update the PID timer
previous_pid_time_us = current_time_us;
}
}
Deconstructing the Code
Let’s walk through the new PID section step-by-step.
- PID Constants (
Kp,Ki,Kd): These are the most important variables in our controller. They determine the “personality” of our three specialists from Session 3. For now, we’ve just put in some placeholder values. 2 - Calculate Error:
error = target_rpm - actual_rpm;. This is the fundamental first step. We find out how far we are from our goal. A positive error means we’re too slow; a negative error means we’re too fast. 3 - Calculate Integral:
integral_error += error * delta_time_s;. We add the current error (scaled by the time since the last calculation) to our running total. This is the “grudge-holding” historian at work, accumulating past errors. 4 - Calculate Derivative:
derivative_error = (error - previous_error) / delta_time_s;. We find the rate of change of the error by seeing how much it changed since the last loop and dividing by the time elapsed. This is our “future-predicting” strategist. 4 - Compute Output:
output = (Kp * error) + (Ki * integral_error) + (Kd * derivative_error);. We sum the contributions of our three specialists, each weighted by its respective gain, to get a final control value. 3 - Constrain and Apply:
motor_power = constrain(output, 0, 255);. The rawoutputcould be any number, butanalogWriteonly accepts values from 0 to 255. Theconstrain()function caps the value within this valid range. This is a simple but effective way to prevent issues like “integral windup,” where the integral term grows to an absurdly large value. 4 Finally, we send this constrained power value to the motor.
What to Expect
Upload the code to your robot and place it on blocks so the wheels can spin freely. Open the Serial Monitor (baud rate 115200).
You should see the wheel spin up and try to settle at 100 RPM. Watch the “Actual RPM” value in the monitor. Does it overshoot 100 RPM and then oscillate back and forth? Does it approach 100 RPM very slowly and never quite get there?
Whatever behavior you see is a direct result of our untuned Kp, Ki, and Kd values. The robot is trying to control itself, but its brain isn’t calibrated yet. It might be too aggressive (high Kp), too focused on the past (high Ki), or too jumpy (high Kd).
What’s Next?
We have done it. We have a working, closed-loop PID speed controller. This is the core of almost all modern motion control.
But a working controller is not the same as a good controller. Our robot’s current performance is likely sluggish, unstable, or inaccurate. The final step in our single-wheel journey is to learn the art and science of PID tuning. In our next session, we will explore how to systematically adjust the Kp, Ki, and Kd gains to transform our robot’s response from clumsy to crisp, fast, and precise.
