In this example, we are going to introduce autonomous state machines. A state machine is a structured way of breaking an autonomous routine into separate steps. Instead of writing one long block of code, we divide the robot’s behaviour into clear states, where each state has one task and one condition for moving to the next state. This is useful because autonomous routines often require the robot to do more than one thing. For example, the robot may need to:  move forward  turn to an angle  move again  stop If all of this is written in one large section of code, it becomes difficult to read, test, and debug. A state machine solves this problem by making the code more organized and easier to understand.

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 the navX 4. stop This is a good introduction to autonomous logic because it shows how the robot can complete one action before moving on to the next.

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;
 // Motor channels
 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;
 // 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:
 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;
 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);
}
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();
 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 so the robot takes the shortest turn
 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();
 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);
 currentState = TURN;
 }
 break;
 case TURN:
 if (std::fabs(angleError) > 2.0)
 {
 double turnSpeed = 0.35;
 if (angleError > 0)
 {
 m0.Set(turnSpeed);
 m1.Set(turnSpeed);
 m2.Set(turnSpeed);
 m3.Set(turnSpeed);
 }
 else
 {
 m0.Set(-turnSpeed);
 m1.Set(-turnSpeed);
 m2.Set(-turnSpeed);
 m3.Set(-turnSpeed);
 }
 }
 else
 {
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
 currentState = STOP;
 }
 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 adding another state after the turn so that the robot moves forward again before stopping.