WIP

Understanding swerve and programming it really goes in opposite directions, but you still need an understanding before attempting to write any code for it.

Swerve is a holonomic drivetrain type which means that commanding motion with swerve does not have constraints like a non-holonomic drivtrain would have. A swerve can both translate and rotation independently at the same time. This makes controlling the robot easy for the driver, but it introduces some hurdles for controlling the drivetrain. Careful planning of your control scheme can make it easier to program and debug later. In this case, swerve can be decomposed into 4 steps.

1. Figure out how you want to move
2. Determine what each module has to do
3. Command the modules to do that
4. Motors to do what they were told by modules

Additionally, it may be helpful to think about what would happen while you are driving. This is important as while we are driving field centric, the robot still has to interpret what it is doing robot centrically. This means that you need to clearly define the front of the robot and which direction is which. It is an easy mistake to get left-right or front-back reversed or even rotating the modules because they're not oriented correctly in the code.

The overall structure will require two classes: SwerveDrive and SwerveModule.

In order to drive, the RobotContainer will interface with the SwerveDrive class. This represents the overall drivetrain and everything it requires. The SwerveDrive class will contain 4 instances of the SwerveModule class.

However, there is a lot of boilerplate code/preparation that has to be done before starting on the actual drive code.

Swerve drives have a lot of motors, encoders, and dimensions. This means that a lot of constants in Constants.h just for the drivetrain. Also, just as a forward, you can use whatever formatting and names you'd like, just keep to good programming habits.

Putting them into one namespace makes this easier to read and use later.

namespace SwerveConstants{ 
    ...
}

Then, the general constants for the entire drivetrain or the values that are the same between drive modules. These include PID constants for the steer and drive motors, the diameter of each wheel, and the gear reduction on the motors.

constexpr double ktP = 0.8; // Turning PID
constexpr double ktI = 0.0;
constexpr double ktD = 0.0;
constexpr double ktFF = 0.0;

constexpr double kdP = 0.1; // Driving Speed PID
constexpr double kdI = 0.0;
constexpr double kdD = 0.0;
constexpr double kdFF = 0.225;

How to tune PID values

constexpr auto maxSpeed = 3.81_mps; // max free speed of SDS mk4 L1 withs neos
constexpr auto maxRotation = 2.0_rad_per_s; // figure this out later
constexpr double driveRatio = 8.14; //SDS Mk4 L1
constexpr double steerRatio = 12.8; //SDS Mk4 L1

constexpr units::inch_t wheelDiameter = 4_in; // likely different based on tread wear
constexpr units::inch_t wheelCircumfrence = units::inch_t{wheelDiameter.value()*MathConstants::pi}; // recalculate based on wheel diameter 
constexpr units::inch_t centerDistance = 10.5_in;
constexpr units::meter_t wheelCircumfrenceMeters = units::meter_t{wheelCircumfrence};//0.31918581_m;

namespace drivebase{
    constexpr units::meter_t WheelBase = 0.5451_m; // for kinematics
    constexpr units::meter_t TrackWidth = 0.5451_m; // aka 21.5 inches
    // 27x27 inch chassis from 2023, will update once 2024 chassis is assembed, but its just +1 inch 
    // now updated to 28x28 2024 chassis
}

Note: some of these values are not nessecary and the way you structure your namespaces is up to you.

Next, you'll need to define the variables that are repeated by different for each module.

namespace topleft{
    constexpr int driveMotor = 9; //CAN ID
    constexpr int steerMotor = 2; //CAN ID
    constexpr int absencoder = 3; //analogin Port

    constexpr double offset = 0.508-0.25;

}

Repeat this for each module with the correct motor ports and offsets, and you're good to go for constants.

Since we are using C++, we first create a .h file that defines all of our variables and functions.

At the most basic level, motors need to follow the commands they are given. Without being sure that the motors will follow commands accurately, the system will not be stable. This is done by commanding the motors using PID controllers. Additionally, it is critical to be sure of the position of the wheel. This is done using a magnetic encoder and the encoders of the motors in the case of our SDS MK4s.

private:

double encoderOffset;

CANSparkMax m_driveMotor;
CANSparkMax m_steerMotor;

SparkRelativeEncoder m_driveEncoder = m_driveMotor.GetEncoder(SparkRelativeEncoder::Type::kHallSensor, 42);
SparkRelativeEncoder m_steerEncoder = m_steerMotor.GetEncoder(SparkRelativeEncoder::Type::kHallSensor, 42);

SparkPIDController m_driveController = m_driveMotor.GetPIDController();
SparkPIDController m_steerController = m_steerMotor.GetPIDController();

frc::AnalogEncoder m_absoluteEncoder;

std::string m_moduleName;

Next, you can create your constructor, helper functions, periodic function, and setState().

public:
SwerveModule(int driveMotor, int steerMotor, int absEncoder, double offset);

void resetModule();
void resetDriveMotor();
void resetSteerMotor();
void resetDriveEncoder();
void resetSteerEncoder();
double getDrivePosition();
double getSteerPosition();
double getDriveVelocity();
double getAbsolutePosition();
std::string getName(int driveMotor);
frc::SwerveModuleState getState();
frc::SwerveModulePosition getPosition();
void setState(const frc::SwerveModuleState state);
void togglePositionOffset(bool toggleOffset);


void Periodic() override;

Now for implementing these functions in the .cpp file, they're mostly just getters and setters, so they should be self-explanatory.

The important part is the setState() function which is what will be called in the SwerveDrive class for each module. This method is what we will use to command the drive and steer motors

frc::SwerveModuleState optimizedState = frc::SwerveModuleState::Optimize(state, units::radian_t(getSteerPosition()));

First, we optimize the state to reduce the amount the steer motor has to move. Since swerve has a ridiculous amount of freedom, instead of rotating say, 120 degrees, you can just rotate 30 degrees in the opposite direction and flip the drive direction. This gets the same result while being much faster.

frc::Rotation2d curAngle = units::radian_t(getAbsolutePosition());

double delta = std::fmod(std::fmod((optimizedState.angle.Radians().value() - curAngle.Radians().value() + pi), pi2) + pi2, pi2) - pi;  // NOLINT

double adjustedAngle = delta + curAngle.Radians().value();

Here we do something to the angle that 2363 did.

Their description says this: "Since we use relative encoder of steer motor, it is a field (doesn't wrap from 2pi to 0 for example). We need to calculate delta to avoid taking a longer route. This is analagous to the EnableContinuousInput() function of WPILib's PIDController classes" - Tony

m_steerController.SetReference((adjustedAngle), CANSparkBase::ControlType::kPosition);

m_driveController.SetReference(optimizedState.speed.value(), CANSparkMax::ControlType::kVelocity);

Finally, we can pass the requested steer angle and drive velocity to each motor's PIDControllers.

Just like in SwerveModule, we need to start with the .h file.

Create an array with the number of swerve modules you are using. Make sure to keep track of which swerve module is which: top left is 0, top right is 1, bottom right is 2, bottom left is 3. Though the order matters when doing kinematics, you can just adjust the order you put them into the kinematics function, so the order here is not as important compared to it making sense to you.

array<SwerveModule, 4> m_modules;

Next, we add kinematics to calculate each module's individual requirements, odometry to keep track of where we've moved, and a pose estimator that returns where we are on the field.

frc::SwerveDriveKinematics<4> m_driveKinematics;
frc::SwerveDriveOdometry<4> m_odometry;
frc::SwerveDrivePoseEstimator<4> m_poseEstimator;

Here is where our swerve implementation gets a little weird. The gyro is initalized in public because otherwise the code crashes due to attempting to call the gyro before it is initalized.

public:

AHRS m_gyro{frc::SPI::Port::kMXP};

Then, we just make the constructor, a bunch of helper functions, and some different driving modes.

SwerveDrive();

frc::Pose2d AveragePose();
frc::Pose2d AveragePose(frc::Pose2d visionPose);
frc::Pose2d OdometryPose();
frc::Rotation2d getGyroHeading();
frc::ChassisSpeeds getRobotRelativeSpeeds();
frc::ChassisSpeeds getFieldRelativeSpeeds();
void resetHeading();
void resetOdometry(const frc::Pose2d pose);
void swerveDrive(double x, double y, double theta, bool fieldCentric);
void swerveDrive(frc::ChassisSpeeds speed);
void brake();
void toggleOffset(bool offset);
bool getOffsetToggle();
void toggleOffset();

void tankDrive(double x, double y);
void moveToAngle(double x, double y);
void resetAbsoluteEncoders();
void SyncAbsoluteEncoders();
void Periodic() override; //update pose using gyro, vision, and odometry

The getters and setters are pretty simple once again. You can see the implementation of each of these modes in the 2024 github repository though there are some caveats I should explain first.

There are two 'swerveDrive' methods: one for teleop using joystick values, and one for autonomous using the ChassisSpeeds that pathplanner makes. They both do essentially the same thing, the teleop version just has to convert the joystick inputs to ChassisSpeeds first. They also discretize (second order kinematics) in different places though we never tested to see which one was correct.

'brake' puts the wheels in an 'o' shape that theoretically (because it's never been tested) makes it hard to push.

'tankDrive' was implemented in 2023, but it wasn't in 2024.

'moveToAngle' is a debugging mode that just drives every wheel in the same direction. It was used to diagnose kinematic issues and determine the swerve module offsets.

So far, we've done swerve in 2023 and 2024, learning a lot everytime we built and coded it. The example code for this page was from the 2024 code.

[2023]

First attempt at swerve.

[2023 Offseason]

Started as an offseason practice project. Ended up as a doomed attempt at rewriting the 2023 code for NTX. The gyro ended up broken and the absolute encoders not implemented correctly, so the wheels have to be reset before starting the robot and the drive is robot-centric. Big thanks to the 6800 mentor who spent almost an entire day helping us fix it.

[2024]

Largely based off the same structure as the 2023 offseason code, but it worked this time.

However, we didn't come up with our swerve code on our own. We used a couple of resources that without them we wouldn't have been able to drive at all.

[364 Base Falcon Swerve]

Main resource for 2023 code

[2363 C++ and Neo Swerve ]

Main resource for 2023 offseason and 2024 code

[0 to autonomous]

code is outdated, but concepts are still good

[YAGSL]

Swerve library, implements some more advanced features, but its best to look at the source code and implement yourself.

[SDS Template]

[REV Template]