Center the gyroscope in the robot, this will help prevent a small drift and ensure more accurate odometry.
Make sure the magnets (if you're using them) are glue'd in right!
Set aside time, assume you will mess up building 1 module or otherwise need a spare during competitions.
Programming a Swerve Drive is hard, and while YAGSL tries to make it easier there are many things you must know to fully understand what you are doing!
Use the right tools for the job! Debugging a Swerve Drive is difficult enough by text only, try out other Dashboards like , or they have excellent visualization tools that are sure to help you out!
The Basics
Swerve Drives move around by moving each wheel to a specific angle/azimuth and rotating the wheel to go in that direction. Swerve Drives are unique because they can rotate independently of their translational movement, meaning you can move in any direction while facing any direction. As a result of the you can "turn in-place" and rotate while moving around an area. The rotation of your robot is referred to as the heading.
What is a Swerve Drive?
A Swerve Drive typically consists of 4 Swerve Modules (which are in essence a drive motor, a angle/azimuth motor, and an absolute encoder), and a gyroscope (centered is best). The motors, absolute encoders, and gyroscope do not matter and can all work together with varying degrees of success. As a rule of thumb if you can stick to one system do it (all REV, all CTRE). This will give you the best feature set however they are not the same! For all other use cases YAGSL is the best choice because YAGSL was built with abstraction in mind to make all sensors and motor controllers functionally equivalent.
TL;DR
A swerve drive is composed of
TL;DR Things that cause issues
This is not a complete list and will grow over time.
How does a Swerve Drive work in code?
SwerveDriveKinematics
SwerveDrive.java
// Import relevant classes.
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
// Example SwerveDrive class
public class SwerveDrive {
// Attributes
SwerveDriveKinematics kinematics;
// Constructor
public SwerveDrive() {
// Create SwerveDriveKinematics object
// 12.5in from center of robot to center of wheel.
// 12.5in is converted to meters to work with object.
// Translation2d(x,y) == Translation2d(front, left)
kinematics = new SwerveDriveKinematics(
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(12.5)), // Front Left
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(-12.5)), // Front Right
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(12.5)), // Back Left
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(-12.5)) // Back Right
);
}
}
The order is defines what the output order of module angle/azimuth's and speeds will be!
SwerveModuleState
SwerveDrive.java
// Import relevant classes.
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.math.kinematics.SwerveModuleState;
// Example SwerveDrive class
public class SwerveDrive
{
// Attributes
SwerveDriveKinematics kinematics;
// Constructor
public SwerveDrive()
{
// Create SwerveDriveKinematics object
// 12.5in from center of robot to center of wheel.
// 12.5in is converted to meters to work with object.
// Translation2d(x,y) == Translation2d(front, left)
kinematics = new SwerveDriveKinematics(
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(12.5)), // Front Left
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(-12.5)), // Front Right
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(12.5)), // Back Left
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(-12.5)) // Back Right
);
}
// Simple drive function
public void drive()
{
// Create test ChassisSpeeds going X = 14in, Y=4in, and spins at 30deg per second.
ChassisSpeeds testSpeeds = new ChassisSpeeds(Units.inchesToMeters(14), Units.inchesToMeters(4), Units.degreesToRadians(30));
// Get the SwerveModuleStates for each module given the desired speeds.
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(testSpeeds);
// Output order is Front-Left, Front-Right, Back-Left, Back-Right
}
}
SwerveDriveOdometry
SwerveDrive.java
// Import relevant classes.
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
// Example SwerveDrive class
public class SwerveDrive extends SubsystemBase
{
// Attributes
SwerveDriveKinematics kinematics;
SwerveDriveOdometry odometry;
Gyroscope gyro; // Psuedo-class representing a gyroscope.
SwerveModule[] swerveModules; // Psuedo-class representing swerve modules.
// Constructor
public SwerveDrive()
{
// Psuedo-code; Create swerve modules here.
// Create SwerveDriveKinematics object
// 12.5in from center of robot to center of wheel.
// 12.5in is converted to meters to work with object.
// Translation2d(x,y) == Translation2d(front, left)
kinematics = new SwerveDriveKinematics(
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(12.5)), // Front Left
new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(-12.5)), // Front Right
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(12.5)), // Back Left
new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(-12.5)) // Back Right
);
gyro = new Gyroscope(); // Psuedo-constructor for generating gyroscope.
// Create the SwerveDriveOdometry given the current angle, the robot is at x=0, r=0, and heading=0
odometry = new SwerveDriveOdometry(
kinematics,
gyro.getAngle(), // returns current gyro reading as a Rotation2d
new SwerveModulePosition[]{new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition},
// Front-Left, Front-Right, Back-Left, Back-Right
new Pose2d(0,0,new Rotation2d()) // x=0, y=0, heading=0
);
}
// Simple drive function
public void drive()
{
// Create test ChassisSpeeds going X = 14in, Y=4in, and spins at 30deg per second.
ChassisSpeeds testSpeeds = new ChassisSpeeds(Units.inchesToMeters(14), Units.inchesToMeters(4), Units.degreesToRadians(30));
// Get the SwerveModuleStates for each module given the desired speeds.
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(testSpeeds);
// Output order is Front-Left, Front-Right, Back-Left, Back-Right
swerveModules[0].setState(swerveModuleStates[0]);
swerveModules[1].setState(swerveModuleStates[1]);
swerveModules[2].setState(swerveModuleStates[2]);
swerveModules[3].setState(swerveModuleStates[3]);
}
// Fetch the current swerve module positions.
public SwerveModulePosition[] getCurrentSwerveModulePositions()
{
return new SwerveModulePosition[]{
new SwerveModulePosition(swerveModules[0].getDistance(), swerveModules[0].getAngle()), // Front-Left
new SwerveModulePosition(swerveModules[1].getDistance(), swerveModules[1].getAngle()), // Front-Right
new SwerveModulePosition(swerveModules[2].getDistance(), swerveModules[2].getAngle()), // Back-Left
new SwerveModulePosition(swerveModules[3].getDistance(), swerveModules[3].getAngle()) // Back-Right
};
}
@Override
public void periodic()
{
// Update the odometry every run.
}
}
SwerveDriveOdometry can be replaced by SwerveDrivePoseEstimator.
Swerve Drive odometry must be updated every single run in a similar fashion to the periodic function from subsystems.
Conclusion
There are many more intricacies to Swerve Drive's than covered on this page however this is sufficient for a basic understanding of how to program a Swerve Drive. I would highly encourage the reader to look at as many examples they can find to understand some of the gotcha's more or continue on to use YAGSL, Good Luck!
Swerve Drives move each module into a specific angle determined by the direction you want to go and heading you want to face. For FRC we can get these value's by hand by calculating the kinematics of the robot or use which uses the module locations to determine what the rotation and speed of each wheel should be given a object and returns an array. The can then be used to set the angle/azimuth and speed corresponding with each Swerve Module to go in the desired direction facing the desired heading.
are used to generate the of each module in the given order, the example below shows how you can do this in the drive() function with a given ChassisSpeeds object.
have 2 properties reflecting the properties of a Swerve Module, angle and speedMetersPerSecond. The goal after getting them is setting the correct swerve module (based on the order given at the construction of the object) to the angle and speed given in the !
Swerve Drive code does not work without or to keep track of module positions and angles!
Unfortunately life isn't that easy. We have to keep continuous track of our current positioning of the robot, specifically the heading, speed, and module positions collectively known as odometry. This is the only way to correctly generate which are usable.
