Refactor code

Author: viggy96

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

@@ -274,8 +274,10 @@ public void set(SwerveModuleState state) {
     // Auto lock modules if auto lock enabled, speed not requested, and time has elapsed
     if (m_autoLock && state.speedMetersPerSecond < EPSILON) {
       state.speedMetersPerSecond = 0.0;
+      // Time's up, lock now...
       if (Duration.between(m_autoLockTimer, Instant.now()).toMillis() > m_autoLockTime)
         state.angle = LOCK_POSITION.minus(m_location.offset);
+      // Waiting to lock...
       else state.angle = m_previousRotatePosition.minus(m_location.offset);
     } else {
       // Not locking this loop, restart timer...

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

@@ -13,8 +13,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 
-/** Turn PID controller */
-public class TurnPIDController extends PIDController {
+/** Rotate PID controller */
+public class RotatePIDController extends PIDController {
   private final double MIN_DEADBAND = 0.001;
   private final double MAX_DEADBAND = 0.2;
   private final double FILTER_FACTOR = 1.0 / 3.0;
@@ -27,14 +27,14 @@ public class TurnPIDController extends PIDController {
   private boolean m_isTurning;
 
   /**
-   * Create an instance of TurnPIDController
-   * @param turnInputCurve Turn input curve
+   * Create an instance of RotatePIDController
+   * @param turnInputCurve Rotate input curve
    * @param pidf PID constants
    * @param turnScalar Value to turn input by (degrees)
    * @param deadband Controller deadband
    * @param lookAhead Number of loops to look ahead by
    */
-  public TurnPIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
+  public RotatePIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
     super(pidf.kP, 0.0, pidf.kD, pidf.period);
     this.m_turnScalar = turnScalar;
     this.m_deadband = MathUtil.clamp(deadband, MIN_DEADBAND, MAX_DEADBAND);

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

@@ -437,8 +437,39 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen
         break;
     }
 
-    m_config.initializeSparkPID(this, selectedSensor, forwardLimitSwitch, reverseLimitSwitch);
-    burnFlash();
+    // Configure feedback sensor and set sensor phase
+    try {
+      m_spark.getPIDController().setFeedbackDevice(selectedSensor);
+      selectedSensor.setInverted(m_config.getSensorPhase());
+    } catch (IllegalArgumentException e) {}
+
+    // Configure forward and reverse soft limits
+    if (config.isSoftLimitEnabled()) {
+      setForwardSoftLimit(config.getUpperLimit());
+      enableForwardSoftLimit();
+      setReverseSoftLimit(config.getLowerLimit());
+      enableReverseSoftLimit();
+    }
+
+    // Configure forward and reverse limit switches if required, and disable soft limit
+    if (forwardLimitSwitch) {
+      enableForwardLimitSwitch();
+      disableForwardSoftLimit();
+    }
+    if (reverseLimitSwitch) {
+      enableReverseLimitSwitch();
+      disableReverseSoftLimit();
+    }
+
+    // Invert motor if required
+    setInverted(config.getInverted());
+
+    // Configure PID values
+    setP(config.getP());
+    setI(config.getI());
+    setD(config.getD());
+    setF(config.getF());
+    setIzone(config.getI() != 0.0 ? config.getTolerance() * 2 : 0.0);
   }
 
   /**
@@ -594,6 +625,83 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac
     return status;
   }
 
+  /**
+   * Set proportional gain for PIDF controller on Spark Max
+   * @param value Value to set
+   * @return {@link REVLibError#kOk} if successful
+   */
+  public REVLibError setP(double value) {
+    REVLibError status;
+    status = applyParameter(
+      () -> m_spark.getPIDController().setP(value),
+      () -> m_spark.getPIDController().getP() == value,
+      "Set kP failure!"
+    );
+    return status;
+  }
+
+  /**
+   * Set integral gain for PIDF controller on Spark Max
+   * @param value Value to set
+   * @return {@link REVLibError#kOk} if successful
+   */
+  public REVLibError setI(double value) {
+    REVLibError status;
+    status = applyParameter(
+      () -> m_spark.getPIDController().setI(value),
+      () -> m_spark.getPIDController().getI() == value,
+      "Set kI failure!"
+    );
+    return status;
+  }
+
+  /**
+   * Set derivative gain for PIDF controller on Spark Max
+   * @param value Value to set
+   * @return {@link REVLibError#kOk} if successful
+   */
+  public REVLibError setD(double value) {
+    REVLibError status;
+    status = applyParameter(
+      () -> m_spark.getPIDController().setD(value),
+      () -> m_spark.getPIDController().getD() == value,
+      "Set kD failure!"
+    );
+    return status;
+  }
+
+  /**
+   * Set feed-forward gain for PIDF controller on Spark Max
+   * @param value Value to set
+   * @return {@link REVLibError#kOk} if successful
+   */
+  public REVLibError setF(double value) {
+    REVLibError status;
+    status = applyParameter(
+      () -> m_spark.getPIDController().setFF(value),
+      () -> m_spark.getPIDController().getFF() == value,
+      "Set kF failure!"
+    );
+    return status;
+  }
+
+  /**
+   * Set integral zone range for PIDF controller on Spark Max
+   * <p>
+   * This value specifies the range the |error| must be within for the integral constant to take effect
+   * @param value Value to set
+   * @return {@link REVLibError#kOk} if successful
+   */
+  public REVLibError setIzone(double value) {
+    REVLibError status;
+    status = applyParameter(
+      () -> m_spark.getPIDController().setIZone(value),
+      () -> m_spark.getPIDController().getIZone() == value,
+      "Set Izone failure!"
+    );
+    return status;
+  }
+
   /**
    * Execute a smooth motion to desired position
    * @param value The target value for the motor

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

@@ -6,9 +6,6 @@
 
 import org.lasarobotics.utils.PIDConstants;
 
-import com.revrobotics.MotorFeedbackSensor;
-import com.revrobotics.SparkMaxPIDController;
-
 /**
  * Automates the configuration of Spark PID
  */
@@ -71,72 +68,6 @@ public SparkPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto
     this.m_enableSoftLimits = enableSoftLimits;
   }
 
-  /**
-   * Initializes Spark Max PID
-   * <p>
-   * Must call {@link SparkMax#burnFlash()} after configuring all settings
-   * @param spark Spark motor controller to apply settings to
-   * @param feedbackSensor Feedback device to use for Spark PID
-   * @param forwardLimitSwitch Enable forward limit switch
-   * @param reverseLimitSwitch Enable reverse limit switch
-   */
-  public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor,
-                                 boolean forwardLimitSwitch, boolean reverseLimitSwitch) {
-    // Get PID controller
-    SparkMaxPIDController pidController = spark.getMotorController().getPIDController();
-
-    // Configure feedback sensor and set sensor phase
-    try {
-      pidController.setFeedbackDevice(feedbackSensor);
-      feedbackSensor.setInverted(m_sensorPhase);
-    } catch (IllegalArgumentException e) {}
-
-    // Configure forward and reverse soft limits
-    if (m_enableSoftLimits) {
-      spark.setForwardSoftLimit(m_upperLimit);
-      spark.enableForwardSoftLimit();
-      spark.setReverseSoftLimit(m_lowerLimit);
-      spark.enableReverseSoftLimit();
-    }
-
-    // Configure forward and reverse limit switches if required, and disable soft limit
-    if (forwardLimitSwitch) {
-      spark.enableForwardLimitSwitch();
-      spark.disableForwardSoftLimit();
-    }
-    if (reverseLimitSwitch) {
-      spark.enableReverseLimitSwitch();
-      spark.disableReverseSoftLimit();
-    }
-
-    // Invert motor if required
-    spark.setInverted(m_invertMotor);
-
-    // Configure PID values
-    spark.applyParameter(() -> pidController.setP(m_kP), () -> pidController.getP() == m_kP, "Set kP failure!");
-    spark.applyParameter(() -> pidController.setI(m_kI), () -> pidController.getI() == m_kI, "Set kI failure!");
-    spark.applyParameter(() -> pidController.setD(m_kD), () -> pidController.getD() == m_kD, "Set kD failure!");
-    spark.applyParameter(() -> pidController.setFF(m_kF), () -> pidController.getFF() == m_kF, "Set kF failure!");
-    spark.applyParameter(
-      () -> pidController.setIZone(m_kI != 0.0 ? m_tolerance * 2 : 0.0),
-      () -> pidController.getIZone() == (m_kI != 0.0 ? m_tolerance * 2 : 0.0),
-      "Set Izone failure!"
-    );
-  }
-
-  /**
-   * Initializes Spark PID
-   * <p>
-   * Calls {@link SparkPIDConfig#initializeSparkPID(SparkMax, MotorFeedbackSensor, boolean, boolean)} with no limit switches
-   * <p>
-   * Must call {@link SparkMax#burnFlash()} after configuring all settings
-   * @param spark Spark motor controller to apply settings to
-   * @param feedbackSensor Feedback device to use for Spark PID
-   */
-  public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor) {
-    initializeSparkPID(spark, feedbackSensor, false, false);
-  }
-
   /**
    * @return Sensor phase
    */
@@ -147,35 +78,35 @@ public boolean getSensorPhase() {
   /**
    * @return Whether motor is inverted or not
    */
-  public boolean getInvertMotor() {
+  public boolean getInverted() {
     return m_invertMotor;
   }
 
   /**
    * @return Proportional gain
    */
-  public double getkP() {
+  public double getP() {
     return m_kP;
   }
 
   /**
    * @return Integral gain
    */
-  public double getkI() {
+  public double getI() {
     return m_kI;
   }
 
   /**
    * @return Derivative gain
    */
-  public double getkD() {
+  public double getD() {
     return m_kD;
   }
 
   /**
    * @return Feed-forward gain
    */
-  public double getkF() {
+  public double getF() {
     return m_kF;
   }
 
@@ -186,6 +117,10 @@ public double getTolerance() {
     return m_tolerance;
   }
 
+  public boolean isSoftLimitEnabled() {
+    return m_enableSoftLimits;
+  }
+
   /**
    * @return Lower limit of mechanism
    */

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

@@ -19,7 +19,7 @@
 import org.lasarobotics.utils.PIDConstants;
 
 @TestMethodOrder(MethodOrderer.OrderAnnotation.class)
-public class TurnPIDControllerTest {
+public class RotatePIDControllerTest {
   private final double DELTA = 1e-5;
 
   private static final double CONTROLLER_DEADBAND = 0.10;
@@ -30,11 +30,11 @@ public class TurnPIDControllerTest {
   private static final double DRIVE_TURN_SCALAR = 30.0;
   private static final double DRIVE_LOOKAHEAD = 3;
 
-  private TurnPIDController m_turnPIDController;
+  private RotatePIDController m_turnPIDController;
 
   @BeforeEach
   public void setup() {
-    m_turnPIDController = new TurnPIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
+    m_turnPIDController = new RotatePIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
   }
 
   @Test