LST Robotics Logo

Learning • Sensors • Tech

LST Robotics

 Lessons
← Back to C++ Coding Track

C++ Coding Track · Lesson 18

Autonomous State Machine with Heading Hold and PID Turning

In this version, we will improve the previous state machine by using PID control inside the TURN state. Instead of turning at a fixed speed, the robot will now measure its heading error and reduce that error smoothly as it approaches the target angle. This is useful because a fixed-speed turn can overshoot or stop inaccurately, especially if the robot is moving quickly or if the floor surface changes. By using PID inside the turning state, the robot can make more precise and stable turns while still keeping the overall autonomous routine organized in a clear step-by-step structure. In this example, the robot will: 1. start in a setup state 2. drive forward to a target distance 3. turn to a target angle using PID 4. stop This is a good example of how different autonomous techniques can be combined into one structured routine.

Version 1 — PID Turn

Constant.h

#pragma once
#define _USE_MATH_DEFINES
#include <math.h>
// Defines a namespace to hold constant values
namespace constant
{
 static constexpr int TITAN_ID = 42;
 static constexpr int M0 = 0;
 static constexpr int M1 = 1;
 static constexpr int M2 = 2;
 static constexpr int M3 = 3;
 static constexpr int frequency = 15600;
 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:
 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};
 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};
 AHRS navx{frc::SPI::Port::kMXP};
 double driveTargetDistance = 20.0;
 double turnTargetAngle = 180.0;
 // PID values for turn
 double kP = 0.015;
 double kI = 0.0000;
 double kD = 0.002;
 double previousError = 0.0;
 double integral = 0.0;
 enum AutoState
 {
 START,
 DRIVE_FORWARD,
 TURN,
 STOP
 };
 AutoState currentState = START;
};

Robot.cpp

#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <cmath>
void Robot::RobotInit()
{
 m0Encoder.SetDistancePerPulse(0.01);
 m1Encoder.SetDistancePerPulse(0.01);
 m2Encoder.SetDistancePerPulse(0.01);
 m3Encoder.SetDistancePerPulse(0.01);
}
void Robot::RobotPeriodic()
{
 double averageDistance =
 (std::fabs(m0Encoder.GetDistance()) +
 std::fabs(m1Encoder.GetDistance()) +
 std::fabs(m2Encoder.GetDistance()) +
 std::fabs(m3Encoder.GetDistance())) / 4.0;
 frc::SmartDashboard::PutNumber("m0Encoder", m0Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m1Encoder", m1Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m2Encoder", m2Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m3Encoder", m3Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("Average Encoder Distance", averageDistance);
 frc::SmartDashboard::PutNumber("navX Yaw", navx.GetYaw());
 frc::SmartDashboard::PutNumber("Auto State", currentState);
 frc::SmartDashboard::PutNumber("Target Angle", turnTargetAngle);
 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;
 currentState = START;
}
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 angleError = turnTargetAngle - currentAngle;
 // Wrap angle error so the robot takes the shortest path
 while (angleError > 180.0) angleError -= 360.0;
 while (angleError < -180.0) angleError += 360.0;
 switch (currentState)
 {
 case START:
 m0Encoder.Reset();
 m1Encoder.Reset();
 m2Encoder.Reset();
 m3Encoder.Reset();
 previousError = 0.0;
 integral = 0.0;
 currentState = DRIVE_FORWARD;
 break;
 case DRIVE_FORWARD:
 if (averageDistance < driveTargetDistance)
 {
 m0.Set(0.5);
 m1.Set(0.5);
 m2.Set(-0.5);
 m3.Set(-0.5);
 }
 else
 {
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 previousError = 0.0;
 integral = 0.0;
 currentState = TURN;
 }
 break;
 case TURN:
 {
 integral += angleError;
 double derivative = angleError - previousError;
 double output = (kP * angleError) + (kI * integral) + (kD * derivative);
 // Limit output so the robot does not turn too aggressively
 if (output > 0.5) output = 0.5;
 if (output < -0.5) output = -0.5;
 frc::SmartDashboard::PutNumber("PID Error", angleError);
 frc::SmartDashboard::PutNumber("PID Integral", integral);
 frc::SmartDashboard::PutNumber("PID Derivative", derivative);
 frc::SmartDashboard::PutNumber("PID Output", output);
 if (std::fabs(angleError) > 2.0)
 {
 m0.Set(output);
 m1.Set(output);
 m2.Set(output);
 m3.Set(output);
 }
 else
 {
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 currentState = STOP;
 }
 previousError = angleError;
 break;
 }
 case STOP:
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 break;
 }
}
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>(); }
#endif

Try changing the driveTargetDistance, turnTargetAngle, kP, kI, and kD values to see how the robot drives forward, turns using PID, and then stops at the final state. Autonomous State Machine with Heading Hold and PID Turning In this example, we will combine heading control, PID turning, and a state machine into one autonomous routine. The robot will begin by driving forward while holding a specific angle. This means that even if the robot starts to drift, the navX will help keep it on the correct heading. After reaching the first target distance, the robot will move into a turning state. In this state, it will use PID control to turn to a new target angle. Once the robot reaches that angle, it will move into a second driving state, where it will again drive forward while holding the new heading for another set distance. This is a very useful example because it shows how a robot can complete a sequence of actions in a structured way while still using sensor feedback to improve accuracy and stability.

Version 2 — Heading Hold and PID Turning

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:
 // Motors
 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
 AHRS navx{frc::SPI::Port::kMXP};
 // Distances
 double firstTargetDistance = 20.0;
 double secondTargetDistance = 20.0;
 // Angles
 double firstDriveAngle = 0.0;
 double turnTargetAngle = 90.0;
 double secondDriveAngle = 90.0;
 // PID values for heading and turning
 double kP = 0.015;
 double kI = 0.0000;
 double kD = 0.002;
 double previousError = 0.0;
 double integral = 0.0;
 enum AutoState
 {
 START,
 DRIVE_FIRST,
 TURN_TO_ANGLE,
 DRIVE_SECOND,
 STOP
 };
 AutoState currentState = START;
};

Robot.cpp

#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <cmath>
void Robot::RobotInit()
{
 m0Encoder.SetDistancePerPulse(0.01);
 m1Encoder.SetDistancePerPulse(0.01);
 m2Encoder.SetDistancePerPulse(0.01);
 m3Encoder.SetDistancePerPulse(0.01);
}
void Robot::RobotPeriodic()
{
 double averageDistance =
 (std::fabs(m0Encoder.GetDistance()) +
 std::fabs(m1Encoder.GetDistance()) +
 std::fabs(m2Encoder.GetDistance()) +
 std::fabs(m3Encoder.GetDistance())) / 4.0;
 frc::SmartDashboard::PutNumber("m0Encoder", m0Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m1Encoder", m1Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m2Encoder", m2Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("m3Encoder", m3Encoder.GetDistance());
 frc::SmartDashboard::PutNumber("Average Encoder Distance", averageDistance);
 frc::SmartDashboard::PutNumber("navX Yaw", navx.GetYaw());
 frc::SmartDashboard::PutNumber("Current State", currentState);
 frc::SmartDashboard::PutNumber("First Drive Angle", firstDriveAngle);
 frc::SmartDashboard::PutNumber("Turn Target Angle", turnTargetAngle);
 frc::SmartDashboard::PutNumber("Second Drive Angle", secondDriveAngle);
 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;
 currentState = START;
}
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 targetAngle = 0.0;
 switch (currentState)
 {
 case START:
 m0Encoder.Reset();
 m1Encoder.Reset();
 m2Encoder.Reset();
 m3Encoder.Reset();
 previousError = 0.0;
 integral = 0.0;
 currentState = DRIVE_FIRST;
 break;
 case DRIVE_FIRST:
 {
 targetAngle = firstDriveAngle;
 double error = targetAngle - currentAngle;
 while (error > 180.0) error -= 360.0;
 while (error < -180.0) error += 360.0;
 integral += error;
 double derivative = error - previousError;
 double correction = (kP * error) + (kI * integral) + (kD * derivative);
 if (correction > 0.3) correction = 0.3;
 if (correction < -0.3) correction = -0.3;
 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;
 frc::SmartDashboard::PutNumber("Heading Error", error);
 frc::SmartDashboard::PutNumber("Heading Correction", correction);
 if (averageDistance < firstTargetDistance)
 {
 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 = 0.0;
 integral = 0.0;
 currentState = TURN_TO_ANGLE;
 }
 previousError = error;
 break;
 }
 case TURN_TO_ANGLE:
 {
 targetAngle = turnTargetAngle;
 double error = targetAngle - currentAngle;
 while (error > 180.0) error -= 360.0;
 while (error < -180.0) error += 360.0;
 integral += error;
 double derivative = error - previousError;
 double output = (kP * error) + (kI * integral) + (kD * derivative);
 if (output > 0.5) output = 0.5;
 if (output < -0.5) output = -0.5;
 frc::SmartDashboard::PutNumber("Turn PID Error", error);
 frc::SmartDashboard::PutNumber("Turn PID Output", output);
 if (std::fabs(error) > 2.0)
 {
 m0.Set(output);
 m1.Set(output);
 m2.Set(output);
 m3.Set(output);
 }
 else
 {
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 m0Encoder.Reset();
 m1Encoder.Reset();
 m2Encoder.Reset();
 m3Encoder.Reset();
 previousError = 0.0;
 integral = 0.0;
 currentState = DRIVE_SECOND;
 }
 previousError = error;
 break;
 }
 case DRIVE_SECOND:
 {
 targetAngle = secondDriveAngle;
 double error = targetAngle - currentAngle;
 while (error > 180.0) error -= 360.0;
 while (error < -180.0) error += 360.0;
 integral += error;
 double derivative = error - previousError;
 double correction = (kP * error) + (kI * integral) + (kD * derivative);
 if (correction > 0.3) correction = 0.3;
 if (correction < -0.3) correction = -0.3;
 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;
 frc::SmartDashboard::PutNumber("Second Heading Error", error);
 frc::SmartDashboard::PutNumber("Second Heading Correction", correction);
 if (averageDistance < secondTargetDistance)
 {
 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);
 currentState = STOP;
 }
 previousError = error;
 break;
 }
 case STOP:
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 break;
 }
}
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>(); }
#endif
← Autonomous State Machines