SwerveModule Class
The SwerveModule class initializes the swerve module on the robot. The method
getState returns a new object of the SwerveModuleState class, allowing us to
know the current state according to the linear speed of the throttle in meters
per second and the degrees the encoder has rotated. The method setState gets
the current heading of the drive and takes a SwerveModuleState object. The
method will use a PID controller to turn the drive to the desired heading at a desired
speed.
Note
For API specific issues, please see the WPILib API , CTRE API, and the REV Robotics API.
Constructor
1// Initialize rotor & throttle motors
2private WPI_TalonFX mRotor;
3private WPI_TalonFX mThrottle;
4
5// Initialize rotor encoder
6private WPI_CANCoder mRotorEncoder;
7
8// Initialize rotor PID controller
9private PIDController mRotorPID;
10
11/**
12 * Construct new SwerveModule
13 *
14 * @param throttleID CAN ID of throttle motor
15 * @param rotorID CAN ID of rotor motor
16 * @param rotorEncoderID CAN ID of rotor encoder
17 * @param rotorOffsetAngleDeg rotor encoder offset
18 */
19public SwerveModule(int throttleID, int rotorID, int rotorEncoderID, double rotorOffsetAngleDeg)
20{
21 // Instantiate throttle motor
22 mThrottle = new WPI_TalonFX(throttleID);
23
24 // Instantiate rotor motor
25 mRotor = new WPI_TalonFX(rotorID);
26
27 // Instantiate rotor absolute encoder
28 mRotorEncoder = new WPI_CANCoder(rotorEncoderID);
29
30 // Reset all configuration
31 // (technically optional, but good practice in case there are old
32 // configurations that can mess with the code)
33 mThrottle.configFactoryDefault();
34 mRotor.configFactoryDefault();
35 mRotorEncoder.configFactoryDefault();
36
37 // Configures rotor motor according to previously defined constants
38 mRotor.setInverted(SwerveConstants.kRotorMotorInversion);
39 mRotor.configVoltageCompSaturation(Constants.kVoltageCompensation);
40 mRotor.enableVoltageCompensation(true);
41 mRotor.setNeutralMode(NeutralMode.Brake);
42
43 // Configures rotor encoder according to previously defined constants
44 mRotorEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
45 mRotorEncoder.configMagnetOffset(rotorOffsetAngleDeg);
46 mRotorEncoder.configSensorDirection(SwerveConstants.kRotorEncoderDirection);
47 mRotorEncoder.configSensorInitializationStrategy(
48 SensorInitializationStrategy.BootToAbsolutePosition
49 );
50
51 // Configures rotor PID controller according to previously defined constants
52 mRotorPID = new PIDController(
53 SwerveConstants.kRotor_kP,
54 SwerveConstants.kRotor_kI,
55 SwerveConstants.kRotor_kD
56 );
57
58 // Continuous input considers the min & max to be the same point and
59 // automatically calculates the shortest route to the setpoint
60 mRotorPID.enableContinuousInput(-180, 180);
61
62 // Configures throttle motor according to previously defined constants
63 mThrottle.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
64 mThrottle.configVoltageCompSaturation(Constants.kVoltageCompensation);
65 mThrottle.enableVoltageCompensation(true);
66 mThrottle.setNeutralMode(NeutralMode.Brake);
67}
1// Initialize rotor & throttle motors
2private CANSparkMax mRotor;
3private CANSparkMax mThrottle;
4
5// Initialize throttle encoder
6private RelativeEncoder mThrottleEncoder;
7
8// Initialize rotor encoder
9private WPI_CANCoder mRotorEncoder;
10
11// Initialize rotor PID controller
12private PIDController mRotorPID;
13
14/**
15 * Construct new SwerveModule
16 *
17 * @param throttleID CAN ID of throttle motor
18 * @param rotorID CAN ID of rotor motor
19 * @param rotorEncoderID CAN ID of rotor encoder
20 * @param rotorOffsetAngleDeg rotor encoder offset
21 */
22public SwerveModule(int throttleID, int rotorID, int rotorEncoderID, double rotorOffsetAngleDeg)
23{
24 // Instantiate throttle motor & respective encoder
25 mThrottle = new CANSparkMax(throttleID, MotorType.kBrushless);
26 mThrottleEncoder = mThrottle.getEncoder();
27
28 // Instantiate rotor motor
29 mRotor = new CANSparkMax(rotorID, MotorType.kBrushless);
30
31 // Instantiate rotor absolute encoder
32 mRotorEncoder = new WPI_CANCoder(rotorEncoderID);
33
34 // Reset all configuration
35 // (technically optional, but good practice in case there are old
36 // configurations that can mess with the code)
37 mThrottle.restoreFactoryDefaults();
38 mRotor.restoreFactoryDefaults();
39 mRotorEncoder.configFactoryDefault();
40
41 // Configures rotor motor according to previously defined constants
42 mRotor.setInverted(SwerveConstants.kRotorMotorInversion);
43 mRotor.enableVoltageCompensation(Constants.kVoltageCompensation);
44 mRotor.setIdleMode(IdleMode.kBrake);
45
46 // Configures rotor encoder according to previously defined constants
47 mRotorEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
48 mRotorEncoder.configMagnetOffset(rotorOffsetAngleDeg);
49 mRotorEncoder.configSensorDirection(SwerveConstants.kRotorEncoderDirection);
50 mRotorEncoder.configSensorInitializationStrategy(
51 SensorInitializationStrategy.BootToAbsolutePosition
52 );
53
54 // Configures rotor PID controller according to previously defined constants
55 mRotorPID = new PIDController(
56 SwerveConstants.kRotor_kP,
57 SwerveConstants.kRotor_kI,
58 SwerveConstants.kRotor_kD
59 );
60
61 // Continuous input considers the min & max to be the same point and
62 // automatically calculates the shortest route to the setpoint
63 mRotorPID.enableContinuousInput(-180, 180);
64
65 // Configures throttle motor according to previously defined constants
66 mThrottle.enableVoltageCompensation(Constants.kVoltageCompensation);
67 mThrottle.setIdleMode(IdleMode.kBrake);
68
69 // Sets conversion factor to throttle encoder so that it reads
70 // velocity in meters per second instead of RPM
71 mThrottleEncoder.setVelocityConversionFactor(
72 SwerveConstants.kThrottleVelocityConversionFactor
73 );
74 // Sets conversion factor to throttle encoder so that it reads
75 // velocity in meters instead of rotations
76 mThrottleEncoder.setPositionConversionFactor(
77 SwerveConstants.kThrottlePositionConversionFactor
78 );
79}
1// Initialize rotor & throttle motors
2private CANSparkMax mRotor;
3private CANSparkMax mThrottle;
4
5// Initialize throttle encoder
6private RelativeEncoder mThrottleEncoder;
7
8// Initialize rotor encoder
9private AnalogPotentiometer mRotorEncoder;
10
11// Initialize rotor PID controller
12private PIDController mRotorPID;
13
14/**
15 * Construct new SwerveModule
16 *
17 * @param throttleID CAN ID of throttle motor
18 * @param rotorID CAN ID of rotor motor
19 * @param rotorEncoderID analog ID of rotor encoder
20 * @param rotorOffsetAngleDeg rotor encoder offset
21 */
22public SwerveModule(int throttleID, int rotorID, int rotorEncoderID, double rotorOffsetAngleDeg)
23{
24 // Instantiate throttle motor & respective encoder
25 mThrottle = new CANSparkMax(throttleID, MotorType.kBrushless);
26 mThrottleEncoder = mThrottle.getEncoder();
27
28 // Instantiate rotor motor
29 mRotor = new CANSparkMax(rotorID, MotorType.kBrushless);
30
31 // Instantiate rotor absolute encoder
32 // - Full range = 360 because that should be the max possible value the
33 // encoder should return
34 mRotorEncoder = new AnalogPotentiometer(rotorEncoderID, 360, rotorOffsetAngleDeg);
35
36 // Reset all configuration
37 // (technically optional, but good practice in case there are old
38 // configurations that can mess with the code)
39 mThrottle.restoreFactoryDefaults();
40 mRotor.restoreFactoryDefaults();
41
42 // Configures rotor motor according to previously defined constants
43 mRotor.setInverted(SwerveConstants.kRotorMotorInversion);
44 mRotor.enableVoltageCompensation(Constants.kVoltageCompensation);
45 mRotor.setIdleMode(IdleMode.kBrake);
46
47 // Configures rotor PID controller according to previously defined constants
48 mRotorPID = new PIDController(
49 SwerveConstants.kRotor_kP,
50 SwerveConstants.kRotor_kI,
51 SwerveConstants.kRotor_kD
52 );
53
54 // Continuous input considers the min & max to be the same point and
55 // automatically calculates the shortest route to the setpoint
56 mRotorPID.enableContinuousInput(-180, 180);
57
58 // Configures throttle motor according to previously defined constants
59 mThrottle.enableVoltageCompensation(Constants.kVoltageCompensation);
60 mThrottle.setIdleMode(IdleMode.kBrake);
61
62 // Sets conversion factor to throttle encoder so that it reads
63 // velocity in meters per second instead of RPM
64 mThrottleEncoder.setVelocityConversionFactor(
65 SwerveConstants.kThrottleVelocityConversionFactor
66 );
67 // Sets conversion factor to throttle encoder so that it reads
68 // velocity in meters instead of rotations
69 mThrottleEncoder.setPositionConversionFactor(
70 SwerveConstants.kThrottlePositionConversionFactor
71 );
72}
Methods
getState
Outputs the current state of the swerve module.
1public SwerveModuleState getState() {
2 double throttleVelocity =
3 mThrottle.getSelectedSensorVelocity() * SwerveConstants.kThrottleVelocityConversionFactor;
4
5 return new SwerveModuleState(
6 throttleVelocity,
7 Rotation2d.fromDegrees(mRotorEncoder.getAbsolutePosition())
8 );
9}
1public SwerveModuleState getState() {
2 return new SwerveModuleState(
3 mThrottleEncoder.getVelocity(),
4 Rotation2d.fromDegrees(mRotorEncoder.getAbsolutePosition())
5 );
6}
Return:
New SwerveModuleState representing the current throttle velocity and rotor angle.
getPosition
Outputs the current position of the swerve module.
1public SwerveModulePosition getPosition() {
2 double throttlePosition = mThrottle.getSelectedSensorPosition() * SwerveConstants.kThrottlePositionConversionFactor;
3
4 return new SwerveModulePosition(
5 throttlePosition,
6 Rotation2d.fromDegrees(mRotorEncoder.getAbsolutePosition())
7 );
8}
1public SwerveModulePosition getPosition() {
2 return new SwerveModulePosition(
3 mThrottleEncoder.getPosition(),
4 Rotation2d.fromDegrees(mRotorEncoder.get())
5 );
6}
Return:
New SwerveModulePosition representing the current throttle position and rotor angle.
setState
Sets the state of the swerve module.
1public void setState(SwerveModuleState state) {
2 // Optimize the desired state so that the rotor doesn't have to spin more than
3 // 90 degrees to get to the desired angle
4 SwerveModuleState optimizedState = SwerveModuleState.optimize(state, getState().angle);
5
6 // Calculate rotor output using rotor PID controller by comparing the current
7 // angle with the desired angle
8 double rotorOutput = mRotorPID.calculate(
9 getState().angle.getDegrees(),
10 optimizedState.angle.getDegrees()
11 );
12
13 mRotor.set(rotorOutput);
14 mThrottle.set(optimizedState.speedMetersPerSecond);
15}
Parameters:
state- desired SwerveModuleState (angle & speed) of SwerveModule
Note
Check out the code we use in these docs on our Github!