-
Notifications
You must be signed in to change notification settings - Fork 34
/
SparkMaxSwerve.java
419 lines (387 loc) · 11.8 KB
/
SparkMaxSwerve.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
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
package swervelib.motors;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
/**
* An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
*/
public class SparkMaxSwerve extends SwerveMotor
{
/**
* SparkMAX Instance.
*/
public CANSparkMax motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public AbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
public SparkMaxPIDController pid;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
factoryDefaults();
clearStickyFaults();
encoder = motor.getEncoder();
pid = motor.getPIDController();
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
// Spin off configurations in a different thread.
configureSparkMax(() -> motor.setCANTimeout(0));
}
/**
* Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
public SparkMaxSwerve(int id, boolean isDriveMotor)
{
this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
}
/**
* Run the configuration until it succeeds or times out.
*
* @param config Lambda supplier returning the error state.
*/
private void configureSparkMax(Supplier<REVLibError> config)
{
for (int i = 0; i < maximumRetries; i++)
{
if (config.get() == REVLibError.kOk)
{
return;
}
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
/**
* Set the voltage compensation for the swerve module motor.
*
* @param nominalVoltage Nominal voltage for operation to output to.
*/
@Override
public void setVoltageCompensation(double nominalVoltage)
{
configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
}
/**
* Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
* voltage compensation. This is useful to protect the motor from current spikes.
*
* @param currentLimit Current limit in AMPS at free speed.
*/
@Override
public void setCurrentLimit(int currentLimit)
{
configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
}
/**
* Set the maximum rate the open/closed loop output can change by.
*
* @param rampRate Time in seconds to go from 0 to full throttle.
*/
@Override
public void setLoopRampRate(double rampRate)
{
configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
}
/**
* Get the motor object from the module.
*
* @return Motor object.
*/
@Override
public Object getMotor()
{
return motor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
{
return absoluteEncoder != null;
}
/**
* Configure the factory defaults.
*/
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
}
/**
* Clear the sticky faults on the motor controller.
*/
@Override
public void clearStickyFaults()
{
configureSparkMax(motor::clearFaults);
}
/**
* Set the absolute encoder to be a compatible absolute encoder.
*
* @param encoder The encoder to use.
* @return The {@link SwerveMotor} for easy instantiation.
*/
@Override
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
{
if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
DriverStation.reportWarning(
"IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
}
return this;
}
/**
* Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
*
* @param positionConversionFactor The conversion factor to apply.
*/
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
if (absoluteEncoder == null)
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
} else
{
configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60));
}
}
/**
* Configure the PIDF values for the closed loop controller.
*
* @param config Configuration class holding the PIDF values.
*/
@Override
public void configurePIDF(PIDFConfig config)
{
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
configureSparkMax(() -> pid.setP(config.p, pidSlot));
configureSparkMax(() -> pid.setI(config.i, pidSlot));
configureSparkMax(() -> pid.setD(config.d, pidSlot));
configureSparkMax(() -> pid.setFF(config.f, pidSlot));
configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
}
/**
* Configure the PID wrapping for the position closed loop controller.
*
* @param minInput Minimum PID input.
* @param maxInput Maximum PID input.
*/
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
/**
* Set the CAN status frames.
*
* @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
* @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
*/
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
* Set the idle mode.
*
* @param isBrakeMode Set the brake mode.
*/
@Override
public void setMotorBrake(boolean isBrakeMode)
{
configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
}
/**
* Set the motor to be inverted.
*
* @param inverted State of inversion.
*/
@Override
public void setInverted(boolean inverted)
{
motor.setInverted(inverted);
}
/**
* Save the configurations from flash to EEPROM.
*/
@Override
public void burnFlash()
{
try
{
Thread.sleep(200);
} catch (Exception e)
{
}
configureSparkMax(() -> motor.burnFlash());
}
/**
* Set the percentage output.
*
* @param percentOutput percent out for the motor controller.
*/
@Override
public void set(double percentOutput)
{
motor.set(percentOutput);
}
/**
* Set the closed loop PID controller reference point.
*
* @param setpoint Setpoint in MPS or Angle in degrees.
* @param feedforward Feedforward in volt-meter-per-second or kV.
*/
@Override
public void setReference(double setpoint, double feedforward)
{
boolean possibleBurnOutIssue = true;
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
if (isDriveMotor)
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kVelocity,
pidSlot,
feedforward));
} else
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kPosition,
pidSlot,
feedforward));
}
}
/**
* Set the closed loop PID controller reference point.
*
* @param setpoint Setpoint in meters per second or angle in degrees.
* @param feedforward Feedforward in volt-meter-per-second or kV.
* @param position Only used on the angle motor, the position of the motor in degrees.
*/
@Override
public void setReference(double setpoint, double feedforward, double position)
{
setReference(setpoint, feedforward);
}
/**
* Get the velocity of the integrated encoder.
*
* @return velocity
*/
@Override
public double getVelocity()
{
return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
}
/**
* Get the position of the integrated encoder.
*
* @return Position
*/
@Override
public double getPosition()
{
return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition();
}
/**
* Set the integrated encoder position.
*
* @param position Integrated encoder position.
*/
@Override
public void setPosition(double position)
{
if (absoluteEncoder == null)
{
configureSparkMax(() -> encoder.setPosition(position));
}
}
/**
* REV Slots for PID configuration.
*/
enum SparkMAX_slotIdx
{
/**
* Slot 1, used for position PID's.
*/
Position,
/**
* Slot 2, used for velocity PID's.
*/
Velocity,
/**
* Slot 3, used arbitrarily. (Documentation show simulations).
*/
Simulation
}
}