PID implementation and tuning Let us now work on PID implementation and tuning. In the previous example, we used simple heading correction to help the robot stay on course. That worked well as an introduction, but in real robotics we often need more accurate and stable control. This is where PID becomes important. PID stands for Proportional, Integral, and Derivative. It is a control method used to reduce error and help the robot reach a target more smoothly and accurately. In this example, we will use the navX IMU to make a simple four-wheel robot turn to a chosen angle. Instead of turning blindly, the robot will measure its current heading, compare it to the target heading, and continuously adjust its motor output until it reaches the correct position.
When working with angles, the robot must always calculate the shortest turning path. This is important because headings wrap around at -180 degrees and 180 degrees. For example, if the target angle is 180 degrees and the robot is currently at -179 degrees, the robot is actually only 1 degree away from the target. However, if we do not correct for angle wrapping, the code may think the error is 359 degrees and make the robot try to turn almost a full circle. To prevent this, we wrap the angle error so it always stays between -180 and 180 degrees, which allows the robot to make the smallest possible correction.
Constant.h
#pragma once
#define _USE_MATH_DEFINES
#include <math.h>
// Defines a namespace to hold constant values
namespace constant
{
// Identifier for the Titan module
static constexpr int TITAN_ID = 42;
// Identifier for the wheel component
static constexpr int M0 = 0;
static constexpr int M1 = 1;
static constexpr int M2 = 2;
static constexpr int M3 = 3;
// Frequency value used in communication
static constexpr int frequency = 15600;
// Array holding port numbers for the wheel encoder channels
static constexpr int M0_VMX[2] = {0,1};
static constexpr int M1_VMX[2] = {2,3};
static constexpr int M2_VMX[2] = {4,5};
static constexpr int M3_VMX[2] = {6,7};
}Robot.h
#pragma once
#include <frc/TimedRobot.h>
#include <frc2/command/Command.h>
#include "RobotContainer.h"
#include <frc/Encoder.h>
#include "studica/TitanQuad.h"
#include "Constants.h"
#include "AHRS.h"
#include <frc/SPI.h>
class Robot : public frc::TimedRobot {- public:
void RobotInit() override;
void RobotPeriodic() override;
void DisabledInit() override;
void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;- private:
// Motor controllers
studica::TitanQuad m0{constant::TITAN_ID, constant::frequency, constant::M0};
studica::TitanQuad m1{constant::TITAN_ID, constant::frequency, constant::M1};
studica::TitanQuad m2{constant::TITAN_ID, constant::frequency, constant::M2};
studica::TitanQuad m3{constant::TITAN_ID, constant::frequency, constant::M3};
// Encoders
frc::Encoder m0Encoder{constant::M0_VMX[0], constant::M0_VMX[1], false, frc::Encoder::k4X};
frc::Encoder m1Encoder{constant::M1_VMX[0], constant::M1_VMX[1], false, frc::Encoder::k4X};
frc::Encoder m2Encoder{constant::M2_VMX[0], constant::M2_VMX[1], false, frc::Encoder::k4X};
frc::Encoder m3Encoder{constant::M3_VMX[0], constant::M3_VMX[1], false, frc::Encoder::k4X};
// navX IMU
AHRS navx{frc::SPI::Port::kMXP};
// Movement targets
double targetDistance = 10.0;
double targetAngle =180.0;
// PID constants
double kP = 0.015;
double kI = 0.0000;
double kD = 0.002;
// PID variables
double previousError = 0.0;
double integral = 0.0;
};Robot.cpp
#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <cmath>
void Robot::RobotInit()
{
// Set how much distance each encoder pulse represents
// Change this value to match your wheel and encoder setup
m0Encoder.SetDistancePerPulse(0.01);
m1Encoder.SetDistancePerPulse(0.01);
m2Encoder.SetDistancePerPulse(0.01);
m3Encoder.SetDistancePerPulse(0.01);
}
void Robot::RobotPeriodic()
{
frc::SmartDashboard::PutNumber("m0Encoder", m0Encoder.GetDistance());
frc::SmartDashboard::PutNumber("m1Encoder", m1Encoder.GetDistance());
frc::SmartDashboard::PutNumber("m2Encoder", m2Encoder.GetDistance());
frc::SmartDashboard::PutNumber("m3Encoder", m3Encoder.GetDistance());
double averageDistance =
(std::fabs(m0Encoder.GetDistance()) +
std::fabs(m1Encoder.GetDistance()) +
std::fabs(m2Encoder.GetDistance()) +
std::fabs(m3Encoder.GetDistance())) / 4.0;
frc::SmartDashboard::PutNumber("Average Encoder Distance", averageDistance);
frc::SmartDashboard::PutNumber("navX Yaw", navx.GetYaw());
frc::SmartDashboard::PutNumber("Target Angle", targetAngle);
frc::SmartDashboard::PutNumber("Target Distance", targetDistance);
frc::SmartDashboard::PutNumber("kP", kP);
frc::SmartDashboard::PutNumber("kI", kI);
frc::SmartDashboard::PutNumber("kD", kD);
}
void Robot::DisabledInit()
{
m0.Set(0.0);
m1.Set(0.0);
m2.Set(0.0);
m3.Set(0.0);
}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit()
{
m0Encoder.Reset();
m1Encoder.Reset();
m2Encoder.Reset();
m3Encoder.Reset();
navx.Reset();
previousError = 0.0;
integral = 0.0;
}
void Robot::AutonomousPeriodic()
{
double averageDistance =
(std::fabs(m0Encoder.GetDistance()) +
std::fabs(m1Encoder.GetDistance()) +
std::fabs(m2Encoder.GetDistance()) +
std::fabs(m3Encoder.GetDistance())) / 4.0;
double currentAngle = navx.GetYaw();
double error = targetAngle - currentAngle;
// Wrap error so robot always takes shortest turn
while (error > 180.0) error -= 360.0;
while (error < -180.0) error += 360.0;
// PID calculations
integral += error;
double derivative = error - previousError;
double correction = (kP * error) + (kI * integral) + (kD * derivative);
// Limit correction so the robot does not turn too aggressively
if (correction > 0.5) correction = 0.5;
if (correction < -0.5) correction = -0.5;
frc::SmartDashboard::PutNumber("PID Error", error);
frc::SmartDashboard::PutNumber("PID Integral", integral);
frc::SmartDashboard::PutNumber("PID Derivative", derivative);
frc::SmartDashboard::PutNumber("PID Correction", correction);
// First turn to the target angle
if (std::fabs(error) > 2.0)
{
m0.Set(correction);
m1.Set(correction);
m2.Set(correction);
m3.Set(correction);
}
// Then move forward while maintaining the angle
else if (averageDistance < targetDistance)
{
double baseSpeed = 0.5;
double leftSpeed = baseSpeed + correction;
double rightSpeed = baseSpeed - correction;
if (leftSpeed > 1.0) leftSpeed = 1.0;
if (leftSpeed < -1.0) leftSpeed = -1.0;
if (rightSpeed > 1.0) rightSpeed = 1.0;
if (rightSpeed < -1.0) rightSpeed = -1.0;
m0.Set(leftSpeed);
m1.Set(leftSpeed);
m2.Set(-rightSpeed);
m3.Set(-rightSpeed);
}
else
{
m0.Set(0.0);
m1.Set(0.0);
m2.Set(0.0);
m3.Set(0.0);
}
previousError = error;
}
void Robot::TeleopInit()
{
m0.Set(0.0);
m1.Set(0.0);
m2.Set(0.0);
m3.Set(0.0);
}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endifTry testing the robot with different target angles such as 90°, 180°, and -90°, and observe how the angle wrapping helps it take the shortest correction path instead of making a full turn.