Swerve Configuration

Before you can start programming the swerve, you need to collect constants specific to your robot that will be used to configure the swerve. All constants will be recorded in Constants.java.

Motor & Sensor IDs

Record the CAN IDs of the rotor and throttle motors.

 1// Rotor IDs
 2public static final int kLeftFrontRotorID = 0;
 3public static final int kRightFrontRotorID = 0;
 4public static final int kLeftRearRotorID = 0;
 5public static final int kRightRearRotorID = 0;
 6
 7// Throttle IDs
 8public static final int kLeftFrontThrottleID = 0;
 9public static final int kRightFrontThrottleID = 0;
10public static final int kLeftRearThrottleID = 0;
11public static final int kRightRearThrottleID = 0;

Record the CAN or analog IDs of the rotor encoders and IMU (not applicable for navX).

1// Rotor Encoder IDs
2public static final int kLeftFrontRotorEncoderID = 0;
3public static final int kRightFrontRotorEncoderID = 0;
4public static final int kLeftRearRotorEncoderID = 0;
5public static final int kRightRearRotorEncoderID = 0;
6
7// IMU ID
8public static final int kImuID = 0;

Rotor Encoder Offsets

In order for the angle of the swerve modules to be in sync, you must compensate for potential rotor encoder offsets.

First, rotate the modules so that all wheels are facing in the same direction. Next, ensure that when positive power is applied to throttle motors, all wheels spin in the forward direction.

../../_images/UpsidedownSwerve.jpg

source

Record the rotor encoder values for each module in the Constants.java file.

1public static final double kLeftFrontRotorOffset = -1 * LEFT_FRONT_ANGLE;
2public static final double kRightFrontRotorOffset = -1 * RIGHT_FRONT_ANGLE;
3public static final double kLeftRearRotorOffset = -1 * LEFT_REAR_ANGLE;
4public static final double kRightRearRotorOffset = -1 * RIGHT_REAR_ANGLE;

Note

The rotor offset will be added to the rotor encoder value. This is why the rotor offset is multiplied by -1.

Rotor Motor & Encoder Inversion

The rotor motor and encoder may need to be inverted. When positive power is applied to the rotor motor, the rotor encoder should increase in value. If this is not the case, you will need to invert the rotor motor or encoder in the Constants.java file by changing the boolean value of the motor/encoder to true.

1public static final SensorDirectionValue kRotorEncoderDirection = SensorDirectionValue.CounterClockwise_Positive;
2public static final InvertedValue kRotorMotorInversion = InvertedValue.CounterClockwise_Positive;

Swerve Kinematics

Width & length of swerve

Record the width and length of the robot (units in meters). Since positive X is forward, and positive Y is left, the configuration should look something like this.

1// Swerve module order: front left, front right, rear left, rear right
2public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics(
3    new Translation2d(LENGTH/2, WIDTH/2),
4    new Translation2d(LENGTH/2, -WIDTH/2),
5    new Translation2d(-LENGTH/2, WIDTH/2),
6    new Translation2d(-LENGTH/2, -WIDTH/2)
7);

Extra Constants

Max Speed/Acceleration

Record the maximum speed and acceleration of the robot (meters).

1public static final double kMaxVelocityMetersPerSecond = 0.0;
2public static final double kMaxAccelerationMetersPerSecond = 0.0;

Wheel Diameter

Record the diameter of the swerve wheels (meters).

1public static final double kWheelDiameterMeters = 0.0;

Throttle Gear Ratio

Record the throttle gear ratio (number of turns it takes the motor to rotate the wheel one revolution).

1public static final double kThrottleGearRatio = 0.0;

Throttle Conversion Constant

Finally, with the constants above, you can calculate the conversion constant that will be used to convert from throttle units to helpful units (meters).

Position Constant

\(conversion = \frac{1}{gear ratio} \times {wheel diameter} \times \pi\)

1public static final double kThrottleVelocityConversionFactor =
2    1/kThrottleGearRatio*kWheelDiameterMeters*Math.PI;

Velocity Constant

\(conversion = \frac{1}{gear ratio} \times \frac{1}{60} \times {wheel diameter} \times \pi\)

Note

The “60” is used to convert from minutes to seconds because SPARK MAX uses rounds per minute for velocity but we want rounds per second

1public static final double kThrottleVelocityConversionFactor =
2    1/kThrottleGearRatio/60*kWheelDiameterMeters*Math.PI;