Swerve Class

The swerve class combines 4 swerve modules to control the swerve as a whole.

Note

For API specific issues, please see the WPILib API , CTRE API, and the REV Robotics API.

Constructor

 1// Initialize IMU
 2private final WPI_Pigeon2 mImu = new WPI_Pigeon2(SwerveConstants.kImuID);
 3
 4private final SwerveModule mLeftFrontModule, mRightFrontModule, mLeftRearModule, mRightRearModule;
 5private SwerveDriveOdometry mOdometry;
 6
 7public Swerve() {
 8    // Instantiate swerve modules - each representing unique module on the robot
 9    mLeftFrontModule = new SwerveModule(
10        SwerveConstants.kLeftFrontThrottleID,
11        SwerveConstants.kLeftFrontRotorID,
12        SwerveConstants.kLeftFrontCANCoderID,
13        SwerveConstants.kLeftFrontRotorOffset
14    );
15
16    mRightFrontModule = new SwerveModule(
17        SwerveConstants.kRightFrontThrottleID,
18        SwerveConstants.kRightFrontRotorID,
19        SwerveConstants.kRightFrontCANCoderID,
20        SwerveConstants.kRightFrontRotorOffset
21    );
22
23    mLeftRearModule = new SwerveModule(
24        SwerveConstants.kLeftRearThrottleID,
25        SwerveConstants.kLeftRearRotorID,
26        SwerveConstants.kLeftRearCANCoderID,
27        SwerveConstants.kLeftRearRotorOffset
28    );
29
30    mRightRearModule = new SwerveModule(
31        SwerveConstants.kRightRearThrottleID,
32        SwerveConstants.kRightRearRotorID,
33        SwerveConstants.kRightRearCANCoderID,
34        SwerveConstants.kRightRearRotorOffset
35    );
36
37    // Instantiate odometry - used for tracking position
38    mOdometry = new SwerveDriveOdometry(
39        SwerveConstants.kSwerveKinematics,
40        mImu.getRotation2d(),
41        getModulePositions()
42    );
43}

Methods

Updates odometry with current module state.

periodic

1@Override
2public void periodic() {
3    // Updates odometry with current module state
4    mOdometry.update(mImu.getRotation2d(), getModulePositions());
5}

drive

Moves chassis in desired direction & rotation.

 1public void drive(double xSpeed, double ySpeed, double zSpeed, boolean fieldOriented) {
 2    SwerveModuleState[] states = null;
 3    if(fieldOriented) {
 4        states = SwerveConstants.kSwerveKinematics.toSwerveModuleStates(
 5            // IMU used for field oriented control
 6            ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, zSpeed, mImu.getRotation2d())
 7        );
 8    } else {
 9        states = SwerveConstants.kSwerveKinematics.toSwerveModuleStates(
10            new ChassisSpeeds(xSpeed, ySpeed, zSpeed)
11        );
12    }
13    setModuleStates(states);
14}

Parameters:

  1. xSpeed - percent power in the X direction

  2. ySpeed - percent power in the Y direction

  3. zSpeed - percent power for rotation

  4. fieldOriented - configure robot movement style (field or robot oriented)

getModuleStates

Outputs the current state of the 4 drive swerve modules.

1public SwerveModuleState[] getModuleStates() {
2    return new SwerveModuleState[]{
3        mLeftFrontModule.getState(),
4        mRightFrontModule.getState(),
5        mLeftRearModule.getState(),
6        mRightRearModule.getState()
7    };
8}

Return:

Return states of all modules in a SwerveModuleState array (Order: [leftFront, rightFront, leftRear, rightRear]).

getModulePositions

Outputs the current positions of the 4 drive swerve modules.

1public SwerveModulePosition[] getModulePositions() {
2    return new SwerveModulePosition[] {
3        mLeftFrontModule.getPosition(),
4        mRightFrontModule.getPosition(),
5        mLeftRearModule.getPosition(),
6        mRightRearModule.getPosition()
7    };
8}

Return:

Return positions of all modules in a SwerveModulePosition array (Order: [leftFront, rightFront, leftRear, rightRear]).

setModuleStates

Sets the state of the 4 drive swerve modules.

1// Swerve module order: [leftFront, leftRear, rightFront, rightRear]
2public void setModuleStates(SwerveModuleState[] desiredStates) {
3    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, 1);
4    mLeftFrontModule.setState(desiredStates[0]);
5    mRightFrontModule.setState(desiredStates[1]);
6    mLeftRearModule.setState(desiredStates[2]);
7    mRightRearModule.setState(desiredStates[3]);
8}

Parameters:

  1. desiredStates - Array of desired SwerveModuleState

getPose

Gets the current position of the robot.

1public Pose2d getPose() {
2    return mOdometry.getPoseMeters();
3}

Return:

New Pose2d representing robot position on the field in meters.

setPose

Sets odometry position to a given x, y, position, and angle.

1public void setPose(Pose2d pose) {
2    mOdometry.resetPosition(mImu.getRotation2d(), getModulePositions(), pose);
3}

Parameters:

  1. pose - A Pose2d object with the robot position and angle

Note

Check out the code we use in these docs on our Github!