This repository has been archived by the owner on Feb 9, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
MAXSwerveModule.java
188 lines (159 loc) · 8.82 KB
/
MAXSwerveModule.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package hardware;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
import com.revrobotics.SparkMaxPIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import calc.Constants.ModuleConstants;
import calc.Constants.NeoMotorConstants;
public class MAXSwerveModule {
private final CANSparkMax drivingSparkMax;
private final CANSparkMax turningSparkMax;
private final RelativeEncoder drivingEncoder;
private final AbsoluteEncoder turningEncoder;
private final SparkMaxPIDController drivingPIDController;
private final SparkMaxPIDController turningPIDController;
private double chassisAngularOffset = 0;
private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d());
/**
* Constructs a MAXSwerveModule and configures the driving and turning motor,
* encoder, and PID controller. This configuration is specific to the REV
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
* Encoder.
*/
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless);
turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless);
// Factory reset, so we get the SPARKS MAX to a known state before configuring
// them. This is useful in case a SPARK MAX is swapped out.
drivingSparkMax.restoreFactoryDefaults();
turningSparkMax.restoreFactoryDefaults();
// Setup encoders and PID controllers for the driving and turning SPARKS MAX.
drivingEncoder = drivingSparkMax.getEncoder();
turningEncoder = turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle);
drivingPIDController = drivingSparkMax.getPIDController();
turningPIDController = turningSparkMax.getPIDController();
drivingPIDController.setFeedbackDevice(drivingEncoder);
turningPIDController.setFeedbackDevice(turningEncoder);
// Apply position and velocity conversion factors for the driving encoder. The
// native units for position and velocity are rotations and RPM, respectively,
// but we want meters and meters per second to use with WPILib's swerve APIs.
drivingEncoder.setPositionConversionFactor(ModuleConstants.DRIVING_ENCODER_POSITION_FACTOR);
drivingEncoder.setVelocityConversionFactor(ModuleConstants.DRIVING_ENCODER_VELOCITY_FACTOR);
// Apply position and velocity conversion factors for the turning encoder. We
// want these in radians and radians per second to use with WPILib's swerve
// APIs.
turningEncoder.setPositionConversionFactor(ModuleConstants.TURNING_ENCODER_POSITION_FACTOR);
turningEncoder.setVelocityConversionFactor(ModuleConstants.TURNING_ENCODER_VELOCITY_FACTOR);
// Invert the turning encoder, since the output shaft rotates in the opposite direction of
// the steering motor in the MAXSwerve Module.
turningEncoder.setInverted(ModuleConstants.TURNING_ENCODER_INVERTED);
// Enable PID wrap around for the turning motor. This will allow the PID
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
// to 10 degrees will go through 0 rather than the other direction which is a
// longer route.
turningPIDController.setPositionPIDWrappingEnabled(true);
turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.TURNING_ENCODER_POSITION_PID_MIN_INPUT);
turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.TURNING_ENCODER_POSITION_PID_MAX_INPUT);
// Set the PID gains for the driving motor. Note these are example gains, and you
// may need to tune them for your own robot!
drivingPIDController.setP(ModuleConstants.DRIVING_P);
drivingPIDController.setI(ModuleConstants.DRIVING_I);
drivingPIDController.setD(ModuleConstants.DRIVING_D);
drivingPIDController.setFF(ModuleConstants.DRIVING_FF);
drivingPIDController.setOutputRange(ModuleConstants.DRIVING_MIN_OUTPUT,
ModuleConstants.DRIVING_MAX_OUTPUT);
// Set the PID gains for the turning motor. Note these are example gains, and you
// may need to tune them for your own robot!
turningPIDController.setP(ModuleConstants.TURNING_P);
turningPIDController.setI(ModuleConstants.TURNING_I);
turningPIDController.setD(ModuleConstants.TURNING_D);
turningPIDController.setFF(ModuleConstants.TURNING_FF);
turningPIDController.setOutputRange(ModuleConstants.TURNING_MIN_OUTPUT,
ModuleConstants.TURNING_MAX_OUTPUT);
drivingSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
turningSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
drivingSparkMax.setSmartCurrentLimit(ModuleConstants.DRIVING_MOTOR_CURRENT_LIMIT);
turningSparkMax.setSmartCurrentLimit(ModuleConstants.TURNING_MOTOR_CURRENT_LIMIT);
// See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-5-default-rate-200ms
drivingSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
turningSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
turningSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20);
// Save the SPARK MAX configurations. If a SPARK MAX browns out during
// operation, it will maintain the above configurations.
NeoMotorConstants.motors.add(drivingSparkMax);
NeoMotorConstants.motors.add(turningSparkMax);
this.chassisAngularOffset = chassisAngularOffset;
desiredState.angle = new Rotation2d(turningEncoder.getPosition());
drivingEncoder.setPosition(0);
}
/**
* Returns the current state of the module.
*
* @return The current state of the module.
*/
public SwerveModuleState getState() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModuleState(drivingEncoder.getVelocity(),
new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset));
}
/**
* Returns the current position of the module.
*
* @return The current position of the module.
*/
public SwerveModulePosition getPosition() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModulePosition(
drivingEncoder.getPosition(),
new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset));
}
/**
* Sets the desired state for the module.
*
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState desiredState) {
// Apply chassis angular offset to the desired state.
SwerveModuleState correctedDesiredState = new SwerveModuleState();
correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(chassisAngularOffset));
// Optimize the reference state to avoid spinning further than 90 degrees.
SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctedDesiredState,
new Rotation2d(turningEncoder.getPosition()));
// Command driving and turning SPARKS MAX towards their respective setpoints.
drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity);
turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition);
this.desiredState = desiredState;
}
/**
* Zeroes all the SwerveModule encoders.
*/
public void resetEncoders() {
drivingEncoder.setPosition(0);
}
/**
* Set the motor to coast mode
*/
public void setCoastMode() {
drivingSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast);
turningSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast);
}
/**
* Set the motor to brake mode
*/
public void setBrakeMode() {
drivingSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
turningSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
}
}