Welcome back to the final leg of our core programming journey. Over the last ten sessions, we’ve built a robot from scratch and painstakingly taught a single wheel how to behave, culminating in a perfectly tuned PID controller that can hold a target speed with remarkable precision.
But a single disciplined soldier doesn’t win the battle. Victory requires a coordinated army.
Today, we conduct the symphony. We will take everything we have learned—kinematics, RPM calculation, and PID control—and weave it all together into a single, master program. We will scale our code from one wheel to four, allowing us to give our robot high-level commands like “move forward,” “strafe left,” and “rotate,” and watch as all four wheels intelligently work together to execute our will.
This is the moment our robot truly becomes a mecanum robot. Let’s write the final code.
The Challenge: Managing Four Brains at Once
The core challenge is one of organization. We can’t just copy and paste our single-wheel PID code four times; that would be a chaotic and unmanageable mess. We need a clean, scalable way to manage four independent PID controllers, each with its own set of pins, gains, and variables.
The solution is to use a struct in C++. A struct is like a custom data container that lets us bundle related variables together. We will create a Motor struct that holds everything a single motor needs to function: its pin numbers, its PID gains, its encoder tick count, its RPM, and its PID error variables.
Then, we can simply create an array of four of these Motor structs. This allows us to write clean, elegant code that loops through each motor to perform the necessary calculations, rather than writing repetitive code for each one.
The Master Plan: Integrating Kinematics
Here is the complete data flow for our final program:
- High-Level Command: We will define our desired robot motion with three simple variables:
target_vx(sideways speed),target_vy(forward/backward speed), andtarget_wz(rotational speed). - Inverse Kinematics: At the start of our control loop, we will feed these three commands into the inverse kinematics equations from Session 4. These equations will calculate the required
target_rpmfor each of the four wheels to achieve the desired overall motion. - Four PID Loops: With a specific
target_rpmfor each wheel, our four independent PID controllers will take over. Each controller will run its own Measure-Compare-Compute-Correct loop, constantly adjusting its motor’s power to ensure itsactual_rpmmatches itstarget_rpm.
This beautiful separation of tasks—kinematics figuring out the goals, and PID controllers executing them—is the heart of advanced robotics motion control.
The Final Code
This is the culmination of our work. It is a long sketch, but it is organized and heavily commented. Take your time to read through it and see how the pieces from our previous sessions fit together.
Before you upload:
- Make sure your pin definitions match your wiring from Session 6.
- Update the
TICKS_PER_WHEEL_REVconstant to match your hardware. - Update the
Kp,Ki, andKdvalues within themotorsarray initialization to the values you found during your tuning session in Session 10.
C++
// --- Pin Definitions ---
// Motor 1: Front-Left
#define ENA_FL 2
#define IN1_FL 22
#define IN2_FL 24
#define ENCODER_A_FL 21
#define ENCODER_B_FL 20
// Motor 2: Rear-Left
#define ENA_RL 3
#define IN1_RL 26
#define IN2_RL 28
#define ENCODER_A_RL 19
#define ENCODER_B_RL 18
// Motor 3: Front-Right
#define ENA_FR 4
#define IN1_FR 30
#define IN2_FR 32
#define ENCODER_A_FR 2 // Note: Using pins 2 & 3 for interrupts
#define ENCODER_B_FR 3
// Motor 4: Rear-Right
#define ENA_RR 5
#define IN1_RR 34
#define IN2_RR 36
#define ENCODER_A_RR 23 // Note: No hardware interrupt, will be handled in software
#define ENCODER_B_RR 25
// --- Hardware & Kinematics Constants ---
const float TICKS_PER_WHEEL_REV = 845.88; // Update with your value
// --- Struct to hold all motor-related variables ---
struct Motor {
// Pins
byte ena_pin;
byte in1_pin;
byte in2_pin;
byte encoder_a_pin;
byte encoder_b_pin;
// PID Gains (tuned in Session 10)
float Kp, Ki, Kd;
// PID Variables
volatile long encoder_ticks;
float target_rpm;
float actual_rpm;
float error;
float integral_error;
float previous_error;
};
// Create an array of 4 motors
Motor motors[1] = {
{ENA_FL, IN1_FL, IN2_FL, ENCODER_A_FL, ENCODER_B_FL, 5.0, 0.5, 0.1}, // FL
{ENA_RL, IN1_RL, IN2_RL, ENCODER_A_RL, ENCODER_B_RL, 5.0, 0.5, 0.1}, // RL
{ENA_FR, IN1_FR, IN2_FR, ENCODER_A_FR, ENCODER_B_FR, 5.0, 0.5, 0.1}, // FR
{ENA_RR, IN1_RR, IN2_RR, ENCODER_A_RR, ENCODER_B_RR, 5.0, 0.5, 0.1} // RR
};
// --- Global Variables for Control ---
unsigned long previous_pid_time_us = 0;
long previous_ticks[1] = {0, 0, 0, 0};
// --- High-Level Robot Commands ---
// Change these values to command the robot
float target_vy = 100.0; // Forward/Backward speed in RPM
float target_vx = 0.0; // Sideways (Strafing) speed in RPM
float target_wz = 0.0; // Rotational speed in RPM
// --- Interrupt Service Routines (ISRs) ---
void readEncoderFL() { if (digitalRead(motors.encoder_b_pin) > 0) motors.encoder_ticks++; else motors.encoder_ticks--; }
void readEncoderRL() { if (digitalRead(motors.[2]encoder_b_pin) > 0) motors.[2]encoder_ticks++; else motors.[2]encoder_ticks--; }
void readEncoderFR() { if (digitalRead(motors.[3]encoder_b_pin) > 0) motors.[3]encoder_ticks++; else motors.[3]encoder_ticks--; }
// Note: No ISR for RR as we ran out of hardware interrupt pins. We'll poll it.
void setup() {
Serial.begin(115200);
for (int i = 0; i < 4; i++) {
pinMode(motors[i].ena_pin, OUTPUT);
pinMode(motors[i].in1_pin, OUTPUT);
pinMode(motors[i].in2_pin, OUTPUT);
pinMode(motors[i].encoder_a_pin, INPUT_PULLUP);
pinMode(motors[i].encoder_b_pin, INPUT_PULLUP);
}
attachInterrupt(digitalPinToInterrupt(motors.encoder_a_pin), readEncoderFL, CHANGE);
attachInterrupt(digitalPinToInterrupt(motors.[2]encoder_a_pin), readEncoderRL, CHANGE);
attachInterrupt(digitalPinToInterrupt(motors.[3]encoder_a_pin), readEncoderFR, CHANGE);
previous_pid_time_us = micros();
}
void loop() {
unsigned long current_time_us = micros();
if (current_time_us - previous_pid_time_us >= 20000) { // 20ms control loop (50 Hz)
float delta_time_s = (current_time_us - previous_pid_time_us) / 1000000.0;
previous_pid_time_us = current_time_us;
// --- 1. Inverse Kinematics ---
// Calculate the target RPM for each wheel based on the desired robot motion
motors.target_rpm = target_vy + target_vx + target_wz; // FL
motors.[2]target_rpm = target_vy - target_vx + target_wz; // RL
motors.[3]target_rpm = target_vy - target_vx - target_wz; // FR
motors.[4]target_rpm = target_vy + target_vx - target_wz; // RR
// --- 2. Loop through each motor to update its PID controller ---
for (int i = 0; i < 4; i++) {
// Calculate actual RPM
long current_ticks = motors[i].encoder_ticks;
float ticks_per_second = (current_ticks - previous_ticks[i]) / delta_time_s;
motors[i].actual_rpm = (ticks_per_second / TICKS_PER_WHEEL_REV) * 60.0;
previous_ticks[i] = current_ticks;
// PID Calculation
motors[i].error = motors[i].target_rpm - motors[i].actual_rpm;
motors[i].integral_error += motors[i].error * delta_time_s;
float derivative_error = (motors[i].error - motors[i].previous_error) / delta_time_s;
motors[i].previous_error = motors[i].error;
float output = (motors[i].Kp * motors[i].error) + (motors[i].Ki * motors[i].integral_error) + (motors[i].Kd * derivative_error);
// Set motor direction and power
int motor_power = abs(output);
motor_power = constrain(motor_power, 0, 255);
if (output > 0) {
digitalWrite(motors[i].in1_pin, HIGH);
digitalWrite(motors[i].in2_pin, LOW);
} else {
digitalWrite(motors[i].in1_pin, LOW);
digitalWrite(motors[i].in2_pin, HIGH);
}
analogWrite(motors[i].ena_pin, motor_power);
}
// Optional: Print debugging info for the first motor
Serial.print("Target: "); Serial.print(motors.target_rpm);
Serial.print(" | Actual: "); Serial.println(motors.actual_rpm);
}
}
Testing Your Creation
Place your robot on the ground. Upload the code. The robot should immediately try to move based on the target_v... values you set at the top of the sketch.
- To move forward: Set
target_vy = 100;and the others to 0. - To strafe right: Set
target_vx = -100;and the others to 0. - To rotate clockwise: Set
target_wz = -50;and the others to 0. - To move diagonally: Set
target_vy = 100;andtarget_vx = -100;.
Experiment with different values. You should see the robot glide smoothly across the floor, with each wheel’s PID controller working tirelessly to maintain its precise target speed.
What’s Next?
You have done it. You have built and programmed a fully functional, closed-loop, PID-controlled mecanum robot. You have navigated the entire journey from basic theory to a complex, intelligent machine.
This is the end of the core curriculum, but it’s just the beginning of your robot’s potential. The master program you’ve just written is a platform. In our final session, we’ll discuss how to build upon this platform by adding remote control capabilities—using a joystick, a phone app, or a controller—to give you real-time command of your creation and truly complete our capstone project.
