Note: While this code intends to provide plug n play configs/tuning numbers for COTS Modules, there are several configs/tunes that are untested with the change to Phoenix 6. If this code is used for one of those untested modules, the module specific configs and tuning will most likely need to be edited.
If anyone would like to contribute a config/tune to be added as "tested" please send a message or make a PR.
Scroll down for a full list of tested/untested configs.
Basic Swerve Code for a Swerve Module using TalonFX Controllers, a CTRE CANCoder, and a CTRE Pigeon Gyro This code was designed with Swerve Drive Specialties MK3, MK4, MK4i, and WCP SwerveX style modules in mind, but should be easily adaptable to other styles of modules.
The following things must be adjusted to your robot and module's specific constants in the Constants.java file (all distance units must be in meters, and rotation units in radians): These instructions are mostly followable from Step
Gyro Settings: pigeonID
(ensure that the gyro rotation is CCW+ (Counter Clockwise Positive)
chosenModule
:
Note: Not every module's configs are tested. See the note at the top
If you are using a supported COTS Module, set the module and drive ratio you are using here.
This will automatically set certain constants for the specific module required to function properly.
If you are not using a COTS supported module, you should delete this variable, and fix all the errors that pop up with correct values for the module you are using
Here is a list of the constants that will automatically be set if you are using a supported module:
trackWidth
: Center to Center distance of left and right modules in meters.
wheelBase
: Center to Center distance of front and rear module wheels in meters.
wheelCircumference
: Cirumference of the wheel (including tread) in meters.
If you are using a supported module, this value will be automatically set.
driveGearRatio
: Total gear ratio for the drive motor.
If you are using a supported module, this value will be automatically set.
angleGearRatio
: Total gear ratio for the angle motor.
If you are using a supported module, this value will be automatically set.
canCoderInvert
and angleMotorInvert
: Both must be set such that they are CCW+.
If you are using a supported module, this value will be automatically set.
driveMotorInvert
: If you are using a supported module, this value will be automatically set.
This can always remain false, since you set your offsets in step 11 such that a positive input to the drive motor will cause the robot to drive forwards.
However this can be set to true if for some reason you prefer the bevel gears on the wheel to face one direction or another when setting offsets. See Step 11 for more information.
Module Specific Constants
: set the Can Id's of the motors and CANCoders for the respective modules, see the next step for setting offsets.
Setting Offsets
driveMotorInvert
value to fix.angleOffset
variable in constants.
angleOffset
you must use Rotation2d.fromDegrees("copied value")
.Angle Motor PID Values:
If you are using a supported module, this value will be automatically set. If you are not, or prefer a more or less aggressive response, you can use the below instructions to tune.
maxSpeed
: In Meters Per Second. maxAngularVelocity
: In Radians Per Second. For these you can use the theoretical values, but it is better to physically drive the robot and find the actual max values.
Get the drive characterization values (KS, KV, KA) by using the WPILib characterization tool, found here. You will need to lock your modules straight forward, and complete the characterization as if it was a standard tank drive.
driveKP
:
After completeing characterization and inserting the KS, KV, and KA values into the code, tune the drive motor kP until it doesn't overshoot and doesnt oscilate around a target velocity.
Leave driveKI
, and driveKD
at 0.0.
This code is natively setup to use a xbox controller to control the swerve drive.
Vendor | Module | Motor | Configs (Motor Inverts etc) | Rotation Motor Tuning |
---|---|---|---|---|
WCP | SwerveX Standard | Kraken X60 | Untested |
Untested |
WCP | SwerveX Standard | Falcon 500 | Untested |
Untested |
WCP | SwerveX Flipped | Kraken X60 | Untested |
Untested |
WCP | SwerveX Flipped | Falcon 500 | Untested |
Untested |
SDS | MK3 | Kraken X60 | Untested |
Untested |
SDS | MK3 | Falcon 500 | Tested |
Untested |
SDS | MK4 | Kraken X60 | Untested |
Untested |
SDS | MK4 | Falcon 500 | Tested |
Untested |
SDS | MK4i | Kraken X60 | Untested |
Untested |
SDS | MK4i | Falcon 500 | Tested |
Tested |
SensorToMechanismRatio
removing the need to convert gear ratios in codeContinuousWrap
removing the need for a custom SwerveModuleState.optimize
function.Swerve
for heading (getGyroYaw()
) to using the odometry output for rotation getHeading()
as intended by WPILib