LST Robotics Logo

Learning • Sensors • Tech

LST Robotics

 Lessons
← Back to C++ Coding Track

C++ Coding Track · Lesson 14

DriveBase Class for Cleaner Code

In this example, we create a separate DriveBase class to handle the robot’s drivetrain movement. Instead of repeatedly writing all four motor commands every time the robot needs to move or stop, we place that logic in one dedicated file and call simple functions such as Stop(), Forward(), Turn(), or DriveWithCorrection(). The DriveBase.h file should be created in the subsystems folder with the other header files, and the DriveBase.cpp file should also be created in the subsystems folder with the other .cpp files. This makes the code much easier to read, easier to update, and less crowded overall. It also helps keep Robot.cpp focused on the robot’s behavior and decision-making, while the DriveBase class handles how the drivetrain actually moves. As programs become larger and more advanced, this structure makes the code cleaner, less packed, and much easier to maintain and debug.

Consstant.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 "Constants.h"
#include "AHRS.h"
#include <frc/SPI.h>
#include "subsystems/DriveBase.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:
 DriveBase drive;
 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 firstTargetDistance = 20.0;
 double secondTargetDistance = 20.0;
 double firstDriveAngle = 0.0;
 double turnTargetAngle = 90.0;
 double secondDriveAngle = 90.0;
 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;
};

DriveBase.h

#pragma once
#include "studica/TitanQuad.h"

#include "Constants.h"
class DriveBase
{
public:
 DriveBase();
 void Stop();
 void Forward(double speed);
 void Turn(double speed);
 void DriveWithCorrection(double leftSpeed, double rightSpeed);
private:
 studica::TitanQuad m0;
 studica::TitanQuad m1;
 studica::TitanQuad m2;
 studica::TitanQuad m3;
};

DriveBase.cpp

#include "subsystems/DriveBase.h"
DriveBase::DriveBase()
 : m0(constant::TITAN_ID, constant::frequency, constant::M0),
 m1(constant::TITAN_ID, constant::frequency, constant::M1),
 m2(constant::TITAN_ID, constant::frequency, constant::M2),
 m3(constant::TITAN_ID, constant::frequency, constant::M3)
{
}
void DriveBase::Stop()
{
 m0.Set(0.0);
 m1.Set(0.0);
 m2.Set(0.0);
 m3.Set(0.0);
}
void DriveBase::Forward(double speed)
{
 m0.Set(speed);
 m1.Set(speed);
 m2.Set(-speed);
 m3.Set(-speed);
}
void DriveBase::Turn(double speed)


{
 m0.Set(speed);
 m1.Set(speed);
 m2.Set(speed);
 m3.Set(speed);
}
void DriveBase::DriveWithCorrection(double leftSpeed, double rightSpeed)
{
 m0.Set(leftSpeed);
 m1.Set(leftSpeed);
 m2.Set(-rightSpeed);
 m3.Set(-rightSpeed);
}

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()
{
 drive.Stop();
}
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)
 {
 m0Encoder.Reset();
 m1Encoder.Reset();
 m2Encoder.Reset();
 m3Encoder.Reset();

 previousError = 0.0;
 integral = 0.0;
 currentState = DRIVE_FIRST;
 break;
 {
 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)
 {
 drive.DriveWithCorrection(leftSpeed, rightSpeed);
 }
 else
 {
 drive.Stop();
 previousError = 0.0;
 integral = 0.0;
 currentState = TURN_TO_ANGLE;
 }

 previousError = error;
 break;
 }
 {
 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)
 {
 drive.Turn(output);
 }
 else
 {
 drive.Stop();
 m0Encoder.Reset();
 m1Encoder.Reset();
 m2Encoder.Reset();
 m3Encoder.Reset();
 previousError = 0.0;
 integral = 0.0;
 currentState = DRIVE_SECOND;
 }
 previousError = error;
 break;
 }
 {
 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)
 {
 drive.DriveWithCorrection(leftSpeed, rightSpeed);
 }
 else
 {
 drive.Stop();
 currentState = STOP;
 }
 previousError = error;
 break;
 }
 drive.Stop();
 break;
 }
}
void Robot::TeleopInit()
{
 drive.Stop();
}

void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

Try adding a new function such as Backward(double speed) or ArcadeDrive(double leftSpeed, double rightSpeed) to the DriveBase class, then use it in Robot.cpp so the robot can perform an extra movement without repeating the motor code. Capstone Integration Project This final project challenges you to combine the knowledge and skills developed throughout the course into one complete robotic system. Instead of following a step-by-step example, you will be required to design, build, program, test, and refine your own solution using the components, control methods, and coding techniques covered in the book. This task will be completed individually, meaning each student will work on their own robot and solution. It is designed as a solo competition, giving every student the opportunity to demonstrate their personal understanding, creativity, and problem-solving ability. The purpose of this project is to encourage independent problem-solving, system integration, and engineering thinking. You must decide how to apply concepts such as drivetrain control, sensors, feedback, autonomous logic, and debugging to create a robot that can successfully complete a defined task. This project is designed to serve as the final demonstration of your understanding and ability to work with a complete robotics system.

← Simple LiDAR Code