Skip to content

Commit 68b40af

Browse files
viggy96viggy96
committed
Refactor code
1 parent 88f1a44 commit 68b40af

File tree

5 files changed

+129
-84
lines changed

5 files changed

+129
-84
lines changed

‎src/main/java/org/lasarobotics/drive/MAXSwerveModule.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -274,8 +274,10 @@ public void set(SwerveModuleState state) {
274274
// Auto lock modules if auto lock enabled, speed not requested, and time has elapsed
275275
if (m_autoLock && state.speedMetersPerSecond < EPSILON) {
276276
state.speedMetersPerSecond = 0.0;
277+
// Time's up, lock now...
277278
if (Duration.between(m_autoLockTimer, Instant.now()).toMillis() > m_autoLockTime)
278279
state.angle = LOCK_POSITION.minus(m_location.offset);
280+
// Waiting to lock...
279281
else state.angle = m_previousRotatePosition.minus(m_location.offset);
280282
} else {
281283
// Not locking this loop, restart timer...

‎src/main/java/org/lasarobotics/drive/TurnPIDController.java‎ renamed to ‎src/main/java/org/lasarobotics/drive/RotatePIDController.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@
1313
import edu.wpi.first.math.MathUtil;
1414
import edu.wpi.first.math.controller.PIDController;
1515

16-
/** Turn PID controller */
17-
public class TurnPIDController extends PIDController {
16+
/** Rotate PID controller */
17+
public class RotatePIDController extends PIDController {
1818
private final double MIN_DEADBAND = 0.001;
1919
private final double MAX_DEADBAND = 0.2;
2020
private final double FILTER_FACTOR = 1.0 / 3.0;
@@ -27,14 +27,14 @@ public class TurnPIDController extends PIDController {
2727
private boolean m_isTurning;
2828

2929
/**
30-
* Create an instance of TurnPIDController
31-
* @param turnInputCurve Turn input curve
30+
* Create an instance of RotatePIDController
31+
* @param turnInputCurve Rotate input curve
3232
* @param pidf PID constants
3333
* @param turnScalar Value to turn input by (degrees)
3434
* @param deadband Controller deadband
3535
* @param lookAhead Number of loops to look ahead by
3636
*/
37-
public TurnPIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
37+
public RotatePIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
3838
super(pidf.kP, 0.0, pidf.kD, pidf.period);
3939
this.m_turnScalar = turnScalar;
4040
this.m_deadband = MathUtil.clamp(deadband, MIN_DEADBAND, MAX_DEADBAND);

‎src/main/java/org/lasarobotics/hardware/revrobotics/SparkMax.java‎

Lines changed: 110 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -437,8 +437,39 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen
437437
break;
438438
}
439439

440-
m_config.initializeSparkPID(this, selectedSensor, forwardLimitSwitch, reverseLimitSwitch);
441-
burnFlash();
440+
// Configure feedback sensor and set sensor phase
441+
try {
442+
m_spark.getPIDController().setFeedbackDevice(selectedSensor);
443+
selectedSensor.setInverted(m_config.getSensorPhase());
444+
} catch (IllegalArgumentException e) {}
445+
446+
// Configure forward and reverse soft limits
447+
if (config.isSoftLimitEnabled()) {
448+
setForwardSoftLimit(config.getUpperLimit());
449+
enableForwardSoftLimit();
450+
setReverseSoftLimit(config.getLowerLimit());
451+
enableReverseSoftLimit();
452+
}
453+
454+
// Configure forward and reverse limit switches if required, and disable soft limit
455+
if (forwardLimitSwitch) {
456+
enableForwardLimitSwitch();
457+
disableForwardSoftLimit();
458+
}
459+
if (reverseLimitSwitch) {
460+
enableReverseLimitSwitch();
461+
disableReverseSoftLimit();
462+
}
463+
464+
// Invert motor if required
465+
setInverted(config.getInverted());
466+
467+
// Configure PID values
468+
setP(config.getP());
469+
setI(config.getI());
470+
setD(config.getD());
471+
setF(config.getF());
472+
setIzone(config.getI() != 0.0 ? config.getTolerance() * 2 : 0.0);
442473
}
443474

444475
/**
@@ -594,6 +625,83 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac
594625
return status;
595626
}
596627

628+
/**
629+
* Set proportional gain for PIDF controller on Spark Max
630+
* @param value Value to set
631+
* @return {@link REVLibError#kOk} if successful
632+
*/
633+
public REVLibError setP(double value) {
634+
REVLibError status;
635+
status = applyParameter(
636+
() -> m_spark.getPIDController().setP(value),
637+
() -> m_spark.getPIDController().getP() == value,
638+
"Set kP failure!"
639+
);
640+
return status;
641+
}
642+
643+
/**
644+
* Set integral gain for PIDF controller on Spark Max
645+
* @param value Value to set
646+
* @return {@link REVLibError#kOk} if successful
647+
*/
648+
public REVLibError setI(double value) {
649+
REVLibError status;
650+
status = applyParameter(
651+
() -> m_spark.getPIDController().setI(value),
652+
() -> m_spark.getPIDController().getI() == value,
653+
"Set kI failure!"
654+
);
655+
return status;
656+
}
657+
658+
/**
659+
* Set derivative gain for PIDF controller on Spark Max
660+
* @param value Value to set
661+
* @return {@link REVLibError#kOk} if successful
662+
*/
663+
public REVLibError setD(double value) {
664+
REVLibError status;
665+
status = applyParameter(
666+
() -> m_spark.getPIDController().setD(value),
667+
() -> m_spark.getPIDController().getD() == value,
668+
"Set kD failure!"
669+
);
670+
return status;
671+
}
672+
673+
/**
674+
* Set feed-forward gain for PIDF controller on Spark Max
675+
* @param value Value to set
676+
* @return {@link REVLibError#kOk} if successful
677+
*/
678+
public REVLibError setF(double value) {
679+
REVLibError status;
680+
status = applyParameter(
681+
() -> m_spark.getPIDController().setFF(value),
682+
() -> m_spark.getPIDController().getFF() == value,
683+
"Set kF failure!"
684+
);
685+
return status;
686+
}
687+
688+
/**
689+
* Set integral zone range for PIDF controller on Spark Max
690+
* <p>
691+
* This value specifies the range the |error| must be within for the integral constant to take effect
692+
* @param value Value to set
693+
* @return {@link REVLibError#kOk} if successful
694+
*/
695+
public REVLibError setIzone(double value) {
696+
REVLibError status;
697+
status = applyParameter(
698+
() -> m_spark.getPIDController().setIZone(value),
699+
() -> m_spark.getPIDController().getIZone() == value,
700+
"Set Izone failure!"
701+
);
702+
return status;
703+
}
704+
597705
/**
598706
* Execute a smooth motion to desired position
599707
* @param value The target value for the motor

‎src/main/java/org/lasarobotics/hardware/revrobotics/SparkPIDConfig.java‎

Lines changed: 9 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,6 @@
66

77
import org.lasarobotics.utils.PIDConstants;
88

9-
import com.revrobotics.MotorFeedbackSensor;
10-
import com.revrobotics.SparkMaxPIDController;
11-
129
/**
1310
* Automates the configuration of Spark PID
1411
*/
@@ -71,72 +68,6 @@ public SparkPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto
7168
this.m_enableSoftLimits = enableSoftLimits;
7269
}
7370

74-
/**
75-
* Initializes Spark Max PID
76-
* <p>
77-
* Must call {@link SparkMax#burnFlash()} after configuring all settings
78-
* @param spark Spark motor controller to apply settings to
79-
* @param feedbackSensor Feedback device to use for Spark PID
80-
* @param forwardLimitSwitch Enable forward limit switch
81-
* @param reverseLimitSwitch Enable reverse limit switch
82-
*/
83-
public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor,
84-
boolean forwardLimitSwitch, boolean reverseLimitSwitch) {
85-
// Get PID controller
86-
SparkMaxPIDController pidController = spark.getMotorController().getPIDController();
87-
88-
// Configure feedback sensor and set sensor phase
89-
try {
90-
pidController.setFeedbackDevice(feedbackSensor);
91-
feedbackSensor.setInverted(m_sensorPhase);
92-
} catch (IllegalArgumentException e) {}
93-
94-
// Configure forward and reverse soft limits
95-
if (m_enableSoftLimits) {
96-
spark.setForwardSoftLimit(m_upperLimit);
97-
spark.enableForwardSoftLimit();
98-
spark.setReverseSoftLimit(m_lowerLimit);
99-
spark.enableReverseSoftLimit();
100-
}
101-
102-
// Configure forward and reverse limit switches if required, and disable soft limit
103-
if (forwardLimitSwitch) {
104-
spark.enableForwardLimitSwitch();
105-
spark.disableForwardSoftLimit();
106-
}
107-
if (reverseLimitSwitch) {
108-
spark.enableReverseLimitSwitch();
109-
spark.disableReverseSoftLimit();
110-
}
111-
112-
// Invert motor if required
113-
spark.setInverted(m_invertMotor);
114-
115-
// Configure PID values
116-
spark.applyParameter(() -> pidController.setP(m_kP), () -> pidController.getP() == m_kP, "Set kP failure!");
117-
spark.applyParameter(() -> pidController.setI(m_kI), () -> pidController.getI() == m_kI, "Set kI failure!");
118-
spark.applyParameter(() -> pidController.setD(m_kD), () -> pidController.getD() == m_kD, "Set kD failure!");
119-
spark.applyParameter(() -> pidController.setFF(m_kF), () -> pidController.getFF() == m_kF, "Set kF failure!");
120-
spark.applyParameter(
121-
() -> pidController.setIZone(m_kI != 0.0 ? m_tolerance * 2 : 0.0),
122-
() -> pidController.getIZone() == (m_kI != 0.0 ? m_tolerance * 2 : 0.0),
123-
"Set Izone failure!"
124-
);
125-
}
126-
127-
/**
128-
* Initializes Spark PID
129-
* <p>
130-
* Calls {@link SparkPIDConfig#initializeSparkPID(SparkMax, MotorFeedbackSensor, boolean, boolean)} with no limit switches
131-
* <p>
132-
* Must call {@link SparkMax#burnFlash()} after configuring all settings
133-
* @param spark Spark motor controller to apply settings to
134-
* @param feedbackSensor Feedback device to use for Spark PID
135-
*/
136-
public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor) {
137-
initializeSparkPID(spark, feedbackSensor, false, false);
138-
}
139-
14071
/**
14172
* @return Sensor phase
14273
*/
@@ -147,35 +78,35 @@ public boolean getSensorPhase() {
14778
/**
14879
* @return Whether motor is inverted or not
14980
*/
150-
public boolean getInvertMotor() {
81+
public boolean getInverted() {
15182
return m_invertMotor;
15283
}
15384

15485
/**
15586
* @return Proportional gain
15687
*/
157-
public double getkP() {
88+
public double getP() {
15889
return m_kP;
15990
}
16091

16192
/**
16293
* @return Integral gain
16394
*/
164-
public double getkI() {
95+
public double getI() {
16596
return m_kI;
16697
}
16798

16899
/**
169100
* @return Derivative gain
170101
*/
171-
public double getkD() {
102+
public double getD() {
172103
return m_kD;
173104
}
174105

175106
/**
176107
* @return Feed-forward gain
177108
*/
178-
public double getkF() {
109+
public double getF() {
179110
return m_kF;
180111
}
181112

@@ -186,6 +117,10 @@ public double getTolerance() {
186117
return m_tolerance;
187118
}
188119

120+
public boolean isSoftLimitEnabled() {
121+
return m_enableSoftLimits;
122+
}
123+
189124
/**
190125
* @return Lower limit of mechanism
191126
*/

‎src/test/java/org/lasarobotics/drive/TurnPIDControllerTest.java‎ renamed to ‎src/test/java/org/lasarobotics/drive/RotatePIDControllerTest.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
import org.lasarobotics.utils.PIDConstants;
2020

2121
@TestMethodOrder(MethodOrderer.OrderAnnotation.class)
22-
public class TurnPIDControllerTest {
22+
public class RotatePIDControllerTest {
2323
private final double DELTA = 1e-5;
2424

2525
private static final double CONTROLLER_DEADBAND = 0.10;
@@ -30,11 +30,11 @@ public class TurnPIDControllerTest {
3030
private static final double DRIVE_TURN_SCALAR = 30.0;
3131
private static final double DRIVE_LOOKAHEAD = 3;
3232

33-
private TurnPIDController m_turnPIDController;
33+
private RotatePIDController m_turnPIDController;
3434

3535
@BeforeEach
3636
public void setup() {
37-
m_turnPIDController = new TurnPIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
37+
m_turnPIDController = new RotatePIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
3838
}
3939

4040
@Test

0 commit comments

Comments
 (0)