Now let's work with the navX and IMU. In this example we use a simple four-wheel robot. The goal isn't to build anything complicated, but to show how the robot can move a set distance while holding a chosen heading.

A common problem in robotics is that a robot may drift slightly left or right while moving. Even with all four motors running, small differences in motor speed, wheel grip, or floor surface can make it veer off course. This is where the navX IMU becomes important.

The navX lets the robot measure its heading, so we can correct its movement while it drives. Here we make the robot move forward while trying to stay locked on a strict 90-degree heading. If it starts drifting, the code automatically adjusts the motor speeds to bring it back on track. This is a simple introduction to heading correction and an important step toward more advanced autonomous movement.

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:
  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 targetDistance = 2000.0;
  double targetAngle = 90.0;
};

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()
{
  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("navX Angle", navx.GetAngle());
}
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();
}
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;
  // Simple proportional correction
  double kP = 0.02;
  double correction = error * kP;
  // Base forward speed
  double baseSpeed = 0.5;
  double leftSpeed = baseSpeed + correction;
  double rightSpeed = baseSpeed - correction;
  // Clamp speeds
  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("Target Angle", targetAngle);
  frc::SmartDashboard::PutNumber("Heading Error", error);
  frc::SmartDashboard::PutNumber("Correction", correction);
  frc::SmartDashboard::PutNumber("Left Speed", leftSpeed);
  frc::SmartDashboard::PutNumber("Right Speed", rightSpeed);
  if (averageDistance < targetDistance)
  {
    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);
  }
}
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

These are the values you can display on your Shuffleboard with this code.

What this code does

• Resets all encoders at the start of autonomous.
• Resets the navX heading.
• Uses the average encoder distance to know how far the robot has travelled.
• Uses the navX yaw to check whether the robot is staying at the target heading.
• Speeds up one side and slows the other if the robot drifts.
• Stops once the target distance is reached.