2.0.1

Fix Lazy Interpolation Gen, Add Filter Interface, and Pose#Mirror for Decode Autos

Author: BaronClaps

core/src/main/java/com/pedropathing/VectorCalculator.java

@@ -182,13 +182,13 @@ public Vector getDriveVector() {
         if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) {
             secondaryDrivePIDF.updateFeedForwardInput(Math.signum(driveError));
             secondaryDrivePIDF.updateError(driveError);
-            driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF(), -maxPowerScaling, maxPowerScaling), tangent.getTheta());
+            driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.run(), -maxPowerScaling, maxPowerScaling), tangent.getTheta());
             return driveVector.copy();
         }
 
         drivePIDF.updateFeedForwardInput(Math.signum(driveError));
         drivePIDF.updateError(driveError);
-        driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF(), -maxPowerScaling, maxPowerScaling), tangent.getTheta());
+        driveVector = new Vector(MathFunctions.clamp(drivePIDF.run(), -maxPowerScaling, maxPowerScaling), tangent.getTheta());
         return driveVector.copy();
     }
 
@@ -207,12 +207,12 @@ public Vector getHeadingVector() {
         if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
             secondaryHeadingPIDF.updateFeedForwardInput(MathFunctions.getTurnDirection(currentPose.getHeading(), headingGoal));
             secondaryHeadingPIDF.updateError(headingError);
-            headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF(), -maxPowerScaling, maxPowerScaling), currentPose.getHeading());
+            headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.run(), -maxPowerScaling, maxPowerScaling), currentPose.getHeading());
             return headingVector.copy();
         }
         headingPIDF.updateFeedForwardInput(MathFunctions.getTurnDirection(currentPose.getHeading(), headingGoal));
         headingPIDF.updateError(headingError);
-        headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF(), -maxPowerScaling, maxPowerScaling), currentPose.getHeading());
+        headingVector = new Vector(MathFunctions.clamp(headingPIDF.run(), -maxPowerScaling, maxPowerScaling), currentPose.getHeading());
         return headingVector.copy();
     }
 
@@ -259,21 +259,21 @@ public Vector getTranslationalCorrection() {
 
         if (currentPose.distanceFrom(closestPose) < translationalPIDFSwitch && useSecondaryTranslationalPID) {
             secondaryTranslationalIntegral.updateError(translationalVector.getMagnitude());
-            secondaryTranslationalIntegralVector = secondaryTranslationalIntegralVector.plus(new Vector(secondaryTranslationalIntegral.runPIDF() - previousSecondaryTranslationalIntegral, translationalVector.getTheta()));
-            previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.runPIDF();
+            secondaryTranslationalIntegralVector = secondaryTranslationalIntegralVector.plus(new Vector(secondaryTranslationalIntegral.run() - previousSecondaryTranslationalIntegral, translationalVector.getTheta()));
+            previousSecondaryTranslationalIntegral = secondaryTranslationalIntegral.run();
 
             secondaryTranslationalPIDF.updateFeedForwardInput(1);
             secondaryTranslationalPIDF.updateError(translationalVector.getMagnitude());
-            translationalVector.setMagnitude(secondaryTranslationalPIDF.runPIDF());
+            translationalVector.setMagnitude(secondaryTranslationalPIDF.run());
             translationalVector = translationalVector.plus(secondaryTranslationalIntegralVector);
         } else {
             translationalIntegral.updateError(translationalVector.getMagnitude());
-            translationalIntegralVector = translationalIntegralVector.plus(new Vector(translationalIntegral.runPIDF() - previousTranslationalIntegral, translationalVector.getTheta()));
-            previousTranslationalIntegral = translationalIntegral.runPIDF();
+            translationalIntegralVector = translationalIntegralVector.plus(new Vector(translationalIntegral.run() - previousTranslationalIntegral, translationalVector.getTheta()));
+            previousTranslationalIntegral = translationalIntegral.run();
 
             translationalPIDF.updateFeedForwardInput(1);
             translationalPIDF.updateError(translationalVector.getMagnitude());
-            translationalVector.setMagnitude(translationalPIDF.runPIDF());
+            translationalVector.setMagnitude(translationalPIDF.run());
             translationalVector = translationalVector.plus(translationalIntegralVector);
         }
 

core/src/main/java/com/pedropathing/control/Controller.java

@@ -0,0 +1,8 @@
+package com.pedropathing.control;
+
+public interface Controller {
+    double run();
+    void reset();
+    void updateError(double error);
+    void updatePosition(double position);
+}

core/src/main/java/com/pedropathing/control/FilteredPIDFController.java

@@ -8,7 +8,7 @@
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 7/15/2024
  */
-public class FilteredPIDFController {
+public class FilteredPIDFController implements Controller{
     private FilteredPIDFCoefficients coefficients;
 
     private double previousError;
@@ -39,7 +39,7 @@ public FilteredPIDFController(FilteredPIDFCoefficients set) {
      *
      * @return this returns the value of the filtered PIDF from the current error.
      */
-    public double runPIDF() {
+    public double run() {
         return error * P() + filteredDerivative * D() + errorIntegral * I() + F() * feedForwardInput;
     }
 

core/src/main/java/com/pedropathing/control/KalmanFilter.java

@@ -6,19 +6,32 @@
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 7/17/2024
  */
-public class KalmanFilter {
+public class KalmanFilter implements NoiseFilter {
     private KalmanFilterParameters parameters;
     private double state;
     private double variance;
     private double kalmanGain;
     private double previousState;
     private double previousVariance;
 
+    /**
+     * This creates a new KalmanFilter from a set of KalmanFilterParameters.
+     * @param parameters the parameters to use.
+     */
     public KalmanFilter(KalmanFilterParameters parameters) {
         this.parameters = parameters;
         reset();
     }
 
+    /**
+     * This creates a new KalmanFilter from a set of KalmanFilterParameters, a starting state,
+     * a starting variance, and a starting Kalman gain.
+     *
+     * @param parameters   the parameters to use.
+     * @param startState   the starting state.
+     * @param startVariance the starting variance.
+     * @param startGain    the starting Kalman gain.
+     */
     public KalmanFilter(KalmanFilterParameters parameters, double startState, double startVariance, double startGain) {
         this.parameters = parameters;
         reset(startState, startVariance, startGain);
@@ -50,6 +63,10 @@ public double getState() {
         return state;
     }
 
+    /**
+     * This method outputs the current state, variance, and Kalman gain of the filter as a string array.
+     * @return A string array containing the current state, variance, and Kalman gain.
+     */
     public String[] output() {
         return new String[]{
                 "State: " + state,

core/src/main/java/com/pedropathing/control/LowPassFilter.java

@@ -0,0 +1,60 @@
+package com.pedropathing.control;
+
+/**
+ * This is the LowPassFilter class. This class implements a simple low-pass filter to smooth out noisy data.
+ *
+ * @author Havish Sripada - 12808 RevAmped Robotics
+ */
+public class LowPassFilter implements NoiseFilter {
+    private final double alpha;
+    private double state;
+    private boolean initialized;
+
+    /**
+     * Constructs a LowPassFilter with the specified alpha value.
+     * @param alpha The smoothing factor (0 [less than] alpha [less than] 1). A higher alpha means more smoothing.
+     */
+    public LowPassFilter(double alpha) {
+        if (alpha <= 0 || alpha >= 1) {
+            throw new IllegalArgumentException("Alpha must be between 0 and 1 (exclusive).");
+        }
+        this.alpha = alpha;
+        this.initialized = false;
+    }
+
+    /**
+     * Updates the filter with new data.
+     * @param updateData The new data to update the filter with.
+     * @param updateProjection The projection of the new data (not used in LowPassFilter).
+     */
+    @Override
+    public void update(double updateData, double updateProjection) {
+        if (!initialized) {
+            state = updateData;
+            initialized = true;
+        } else {
+            state = alpha * updateData + (1 - alpha) * state;
+        }
+    }
+
+    /**
+     * Gets the current state of the filter.
+     * @return The current state of the filter.
+     */
+    @Override
+    public double getState() {
+        return state;
+    }
+
+    /**
+     * Resets the filter to a specified state.
+     * @param startState The state to reset the filter to.
+     * @param startVariance The variance to reset the filter to (not used in LowPassFilter).
+     * @param startGain The gain to reset the filter to (not used in LowPassFilter).
+     */
+    @Override
+    public void reset(double startState, double startVariance, double startGain) {
+        this.state = startState;
+        this.initialized = true;
+    }
+}

core/src/main/java/com/pedropathing/control/NoiseFilter.java

@@ -0,0 +1,36 @@
+package com.pedropathing.control;
+
+/**
+ * This is the NoiseFilter interface. This interface defines the methods that any noise filter should implement.
+ *
+ * @author Havish Sripada - 12808 RevAmped Robotics
+ */
+public interface NoiseFilter {
+    /**
+     * Gets the current state of the filter.
+     * @return the current state of the filter
+     */
+    double getState();
+
+    /**
+     * Updates the filter with new data.
+     * @param updateData the new data to update the filter with
+     * @param updateProjection the projection of the new data
+     */
+    void update(double updateData, double updateProjection);
+
+    /**
+     * Resets the filter to its initial state.
+     */
+    default void reset() {
+        reset(0, 1, 1);
+    };
+
+    /**
+     * Resets the filter to a specified state.
+     * @param startState the state to reset the filter to
+     * @param startVariance the variance to reset the filter to
+     * @param startGain the gain to reset the filter to
+     */
+    void reset(double startState, double startVariance, double startGain);
+}

core/src/main/java/com/pedropathing/control/PIDFController.java

@@ -12,7 +12,7 @@
  * @author Harrison Womack - 10158 Scott's Bots
  * @version 1.0, 3/5/2024
  */
-public class PIDFController {
+public class PIDFController implements Controller {
     private PIDFCoefficients coefficients;
 
     private double previousError;
@@ -41,7 +41,7 @@ public PIDFController(PIDFCoefficients set) {
      *
      * @return this returns the value of the PIDF from the current error.
      */
-    public double runPIDF() {
+    public double run() {
         return error * P() + errorDerivative * D() + errorIntegral * I() + feedForwardInput * F();
     }
 
@@ -50,12 +50,12 @@ public double runPIDF() {
      * a target position to calculate error. This will update the error from the current position to
      * the target position specified.
      *
-     * @param update This is the current position.
+     * @param position This is the current position.
      */
-    public void updatePosition(double update) {
-        position = update;
+    public void updatePosition(double position) {
+        this.position = position;
         previousError = error;
-        error = targetPosition - position;
+        error = targetPosition - this.position;
 
         deltaTimeNano = System.nanoTime() - previousUpdateTimeNano;
         previousUpdateTimeNano = System.nanoTime();
@@ -221,4 +221,12 @@ public double F() {
     public double getError() {
         return error;
     }
+
+    /**
+     * This returns the current derivative of the error.
+     * @return the derivative
+     */
+    public double getErrorDerivative() {
+        return errorDerivative;
+    }
 }

core/src/main/java/com/pedropathing/follower/Follower.java

@@ -1040,4 +1040,8 @@ private void setPath(Path path) {
     public PathPoint getPreviousClosestPose() {
         return previousClosestPose;
     }
+
+    public double getHeading() {
+        return getPose().getHeading();
+    }
 }

core/src/main/java/com/pedropathing/geometry/BezierCurve.java

@@ -20,6 +20,7 @@
  * @author Anyi Lin - 10158 Scott's Bots
  * @author Aaron Yang - 10158 Scott's Bots
  * @author Harrison Womack - 10158 Scott's Bots
+ * @author William Phomphakdee - 7462 Not to Scale Alumni
  * @version 1.0, 3/5/2024
  */
 public class BezierCurve implements Curve {