updated to the 9.2 FTC SDK and working on adding the kalman filter and filtered pidf to the drive pid

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -11,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallHeadingPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorKalmanGain;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.tunedDriveErrorVariance;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -34,6 +36,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.DashboardPoseTracker;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.FilteredPIDFController;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilter;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController;
 
 import java.util.ArrayList;
@@ -118,9 +122,14 @@ public class Follower {
     private PIDFController largeTranslationalIntegral = new PIDFController(FollowerConstants.largeTranslationalIntegral);
     private PIDFController smallHeadingPIDF = new PIDFController(FollowerConstants.smallHeadingPIDFCoefficients);
     private PIDFController largeHeadingPIDF = new PIDFController(FollowerConstants.largeHeadingPIDFCoefficients);
-    private PIDFController smallDrivePIDF = new PIDFController(FollowerConstants.smallDrivePIDFCoefficients);
-    private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients);
+    private FilteredPIDFController smallDrivePIDF = new FilteredPIDFController(FollowerConstants.smallDrivePIDFCoefficients);
+    private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients);
 
+    private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
+    private long[] driveErrorTimes;
+    private double[] driveErrors;
+    private double rawDriverError;
+    private double previousRawDriverError;
 
     public static boolean drawOnDashboard = true;
     public static boolean useTranslational = true;
@@ -580,6 +589,14 @@ public class Follower {
         correctiveVector = new Vector();
         driveError = 0;
         headingError = 0;
+        rawDriverError = 0;
+        previousRawDriverError = 0;
+        driveErrors = new double[3];
+        driveErrorTimes = new long[3];
+        for (int i = 0; i < driveErrorTimes.length; i++) {
+            driveErrorTimes[i] = System.currentTimeMillis();
+        }
+        driveKalmanFilter.reset(0, tunedDriveErrorVariance, tunedDriveErrorKalmanGain);
 
         for (int i = 0; i < motors.size(); i++) {
             motors.get(i).setPower(0);
@@ -665,7 +682,26 @@ public class Follower {
         Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta());
         Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError);
 
-        return velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
+        previousRawDriverError = rawDriverError;
+        rawDriverError =  velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
+
+        double previousErrorVelocity = (driveErrors[1] - driveErrors[0]) / ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0);
+        double errorVelocity = (driveErrors[2] - driveErrors[1]) / ((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0);
+        double errorAcceleration = ((errorVelocity - previousErrorVelocity) / ((((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) / 2.0) - (((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0) / 2.0)));
+        double time = (((driveErrorTimes[2] - driveErrorTimes[1]) / 1000.0) + ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0)) / 2.0;
+
+        double projection = errorVelocity * time + 0.5 * errorAcceleration * Math.pow(time, 2);
+
+        driveKalmanFilter.update(rawDriverError - previousRawDriverError, projection);
+
+        for (int i = 0; i < driveErrors.length - 1; i++) {
+            driveErrors[i] = driveErrors[i + 1];
+            driveErrorTimes[i] = driveErrorTimes[i + 1];
+        }
+        driveErrors[2] = driveKalmanFilter.getState();
+        driveErrorTimes[2] = System.currentTimeMillis();
+
+        return driveKalmanFilter.getState();
     }
 
     /**
@@ -890,6 +926,7 @@ public class Follower {
         telemetry.addData("total heading", poseUpdater.getTotalHeading());
         telemetry.addData("velocity magnitude", getVelocity().getMagnitude());
         telemetry.addData("velocity heading", getVelocity().getTheta());
+        driveKalmanFilter.debug(telemetry);
         telemetry.update();
         if (drawOnDashboard) {
             Drawing.drawDebug(this);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -2,10 +2,12 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 
+import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters;
 
 /**
  * This is the FollowerConstants class. It holds many constants and parameters for various parts of
@@ -88,10 +90,11 @@ public class FollowerConstants {
     public static double smallHeadingPIDFFeedForward = 0.01;
 
     // Large drive PIDF coefficients
-    public static CustomPIDFCoefficients largeDrivePIDFCoefficients = new CustomPIDFCoefficients(
+    public static CustomFilteredPIDFCoefficients largeDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
             0.025,
             0,
             0.00001,
+            0.01,
             0);
 
     // Feed forward constant added on to the large drive PIDF
@@ -101,15 +104,25 @@ public class FollowerConstants {
     public static double drivePIDFSwitch = 20;
 
     // Small drive PIDF coefficients
-    public static CustomPIDFCoefficients smallDrivePIDFCoefficients = new CustomPIDFCoefficients(
+    public static CustomFilteredPIDFCoefficients smallDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
             0.02,
             0,
             0.000005,
+            0.01,
             0);
 
     // Feed forward constant added on to the small drive PIDF
     public static double smallDrivePIDFFeedForward = 0.01;
 
+    // Kalman filter parameters for the drive error Kalman filter
+    public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
+            0.4,
+            0.1);
+
+    // These are the empirically tuned parameters for the drive error Kalman filter so it works faster.
+    public static double tunedDriveErrorVariance = 1;
+    public static double tunedDriveErrorKalmanGain = 1;
+
     // Mass of robot in kilograms
     public static double mass = 10.65942;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/CustomFilteredPIDFCoefficients.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.util;
+
+import kotlin.jvm.JvmField;
+
+/**
+ * This is the CustomFilteredPIDFCoefficients class. This class handles holding coefficients for filtered PIDF
+ * controllers.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 7/15/2024
+ */
+public class CustomFilteredPIDFCoefficients {
+    @JvmField public double P;
+    @JvmField public double I;
+    @JvmField public double D;
+    @JvmField public double T;
+    @JvmField public double F;
+
+    public FeedForwardConstant feedForwardConstantEquation;
+
+    private boolean usingEquation;
+
+    /**
+     * This creates a new CustomFilteredPIDFCoefficients with constant coefficients.
+     *
+     * @param p the coefficient for the proportional factor.
+     * @param i the coefficient for the integral factor.
+     * @param d the coefficient for the derivative factor.
+     * @param t the time constant for the filter
+     * @param f the coefficient for the feedforward factor.
+     */
+    public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, double f) {
+        P = p;
+        I = i;
+        D = d;
+        T = t;
+        F = f;
+    }
+
+    /**
+     * This creates a new CustomFilteredPIDFCoefficients with constant PID coefficients and a variable
+     * feedforward equation using a FeedForwardConstant.
+     *
+     * @param p the coefficient for the proportional factor.
+     * @param i the coefficient for the integral factor.
+     * @param d the coefficient for the derivative factor.
+     * @param t the time constant for the filter
+     * @param f the equation for the feedforward factor.
+     */
+    public CustomFilteredPIDFCoefficients(double p, double i, double d, double t, FeedForwardConstant f) {
+        usingEquation = true;
+        P = p;
+        I = i;
+        D = d;
+        T = t;
+        feedForwardConstantEquation = f;
+    }
+
+    /**
+     * This returns the coefficient for the feedforward factor.
+     *
+     * @param input this is inputted into the feedforward equation, if applicable. If there's no
+     *              equation, then any input can be used.
+     * @return This returns the coefficient for the feedforward factor.
+     */
+    public double getCoefficient(double input) {
+        if (!usingEquation) return F;
+        return feedForwardConstantEquation.getConstant(input);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/FilteredPIDFController.java

@@ -0,0 +1,245 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.util;
+
+/**
+ * This is the FilteredPIDFController class. This class handles the running of filtered filtered PIDFs. This
+ * behaves very similarly to a regular filtered PIDF controller, but the derivative portion is filtered with
+ * a low pass filter to reduce high frequency noise that could affect results.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 7/15/2024
+ */
+public class FilteredPIDFController {
+    private CustomFilteredPIDFCoefficients coefficients;
+
+    private double previousError;
+    private double error;
+    private double position;
+    private double targetPosition;
+    private double errorIntegral;
+    private double errorDerivative;
+    private double previousDerivative;
+    private double filteredDerivative;
+    private double feedForwardInput;
+
+    private long previousUpdateTimeNano;
+    private long deltaTimeNano;
+
+    /**
+     * This creates a new filtered PIDFController from a CustomPIDFCoefficients.
+     *
+     * @param set the coefficients to use.
+     */
+    public FilteredPIDFController(CustomFilteredPIDFCoefficients set) {
+        setCoefficients(set);
+        reset();
+    }
+
+    /**
+     * This takes the current error and runs the filtered PIDF on it.
+     *
+     * @return this returns the value of the filtered PIDF from the current error.
+     */
+    public double runPIDF() {
+        return error * P() + filteredDerivative * D() + errorIntegral * I() + F();
+    }
+
+    /**
+     * This can be used to update the filtered PIDF's current position when inputting a current position and
+     * 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.
+     */
+    public void updatePosition(double update) {
+        position = update;
+        previousError = error;
+        error = targetPosition - position;
+
+        deltaTimeNano = System.nanoTime() - previousUpdateTimeNano;
+        previousUpdateTimeNano = System.nanoTime();
+
+        errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9));
+        previousDerivative = filteredDerivative;
+        errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9));
+        filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative;
+    }
+
+    /**
+     * As opposed to updating position against a target position, this just sets the error to some
+     * specified value.
+     *
+     * @param error The error specified.
+     */
+    public void updateError(double error) {
+        previousError = this.error;
+        this.error = error;
+
+        deltaTimeNano = System.nanoTime() - previousUpdateTimeNano;
+        previousUpdateTimeNano = System.nanoTime();
+
+        errorIntegral += error * (deltaTimeNano / Math.pow(10.0, 9));
+        previousDerivative = errorDerivative;
+        errorDerivative = (error - previousError) / (deltaTimeNano / Math.pow(10.0, 9));
+        filteredDerivative = T() * previousDerivative + (1 - T()) * errorDerivative;
+    }
+
+    /**
+     * This can be used to update the feedforward equation's input, if applicable.
+     *
+     * @param input the input into the feedforward equation.
+     */
+    public void updateFeedForwardInput(double input) {
+        feedForwardInput = input;
+    }
+
+    /**
+     * This resets all the filtered PIDF's error and position values, as well as the time stamps.
+     */
+    public void reset() {
+        previousError = 0;
+        error = 0;
+        position = 0;
+        targetPosition = 0;
+        errorIntegral = 0;
+        errorDerivative = 0;
+        previousDerivative = 0;
+        filteredDerivative = 0;
+        previousUpdateTimeNano = System.nanoTime();
+    }
+
+    /**
+     * This is used to set the target position if the filtered PIDF is being run with current position and
+     * target position inputs rather than error inputs.
+     *
+     * @param set this sets the target position.
+     */
+    public void setTargetPosition(double set) {
+        targetPosition = set;
+    }
+
+    /**
+     * This returns the target position of the filtered PIDF.
+     *
+     * @return this returns the target position.
+     */
+    public double getTargetPosition() {
+        return targetPosition;
+    }
+
+    /**
+     * This is used to set the coefficients of the filtered PIDF.
+     *
+     * @param set the coefficients that the filtered PIDF will use.
+     */
+    public void setCoefficients(CustomFilteredPIDFCoefficients set) {
+        coefficients = set;
+    }
+
+    /**
+     * This returns the filtered PIDF's current coefficients.
+     *
+     * @return this returns the current coefficients.
+     */
+    public CustomFilteredPIDFCoefficients getCoefficients() {
+        return coefficients;
+    }
+
+    /**
+     * This sets the proportional (P) coefficient of the filtered PIDF only.
+     *
+     * @param set this sets the P coefficient.
+     */
+    public void setP(double set) {
+        coefficients.P = set;
+    }
+
+    /**
+     * This returns the proportional (P) coefficient of the filtered PIDF.
+     *
+     * @return this returns the P coefficient.
+     */
+    public double P() {
+        return coefficients.P;
+    }
+
+    /**
+     * This sets the integral (I) coefficient of the filtered PIDF only.
+     *
+     * @param set this sets the I coefficient.
+     */
+    public void setI(double set) {
+        coefficients.I = set;
+    }
+
+    /**
+     * This returns the integral (I) coefficient of the filtered PIDF.
+     *
+     * @return this returns the I coefficient.
+     */
+    public double I() {
+        return coefficients.I;
+    }
+
+    /**
+     * This sets the derivative (D) coefficient of the filtered PIDF only.
+     *
+     * @param set this sets the D coefficient.
+     */
+    public void setD(double set) {
+        coefficients.D = set;
+    }
+
+    /**
+     * This returns the derivative (D) coefficient of the filtered PIDF.
+     *
+     * @return this returns the D coefficient.
+     */
+    public double D() {
+        return coefficients.D;
+    }
+
+    /**
+     * This sets the time constant (T) of the filtered PIDF only.
+     *
+     * @param set this sets the time constant.
+     */
+    public void setT(double set) {
+        coefficients.T = set;
+    }
+
+    /**
+     * This returns the time constant (T) of the filtered PIDF.
+     *
+     * @return this returns the time constant.
+     */
+    public double T() {
+        return coefficients.T;
+    }
+
+    /**
+     * This sets the feedforward (F) constant of the filtered PIDF only.
+     *
+     * @param set this sets the F constant.
+     */
+    public void setF(double set) {
+        coefficients.F = set;
+    }
+
+    /**
+     * This returns the feedforward (F) constant of the filtered PIDF.
+     *
+     * @return this returns the F constant.
+     */
+    public double F() {
+        return coefficients.getCoefficient(feedForwardInput);
+    }
+
+    /**
+     * This returns the current error of the filtered PIDF.
+     *
+     * @return this returns the error.
+     */
+    public double getError() {
+        return error;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilter.java

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.util;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+/**
+ * This is the KalmanFilter class. This creates a Kalman filter that is used to smooth out data.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 7/17/2024
+ */
+public class KalmanFilter {
+    private KalmanFilterParameters parameters;
+    private double state;
+    private double variance;
+    private double kalmanGain;
+    private double previousState;
+    private double previousVariance;
+
+    public KalmanFilter(KalmanFilterParameters parameters) {
+        this.parameters = parameters;
+        reset();
+    }
+
+    public KalmanFilter(KalmanFilterParameters parameters, double startState, double startVariance, double startGain) {
+        this.parameters = parameters;
+        reset(startState, startVariance, startGain);
+    }
+
+    public void reset(double startState, double startVariance, double startGain) {
+        state = startState;
+        previousState = startState;
+        variance = startVariance;
+        previousVariance = startVariance;
+        kalmanGain = startGain;
+    }
+
+    public void reset() {
+        reset(0, 1, 1);
+    }
+
+    public void update(double updateData, double updateProjection) {
+        state = previousState + updateData;
+        variance = previousVariance + parameters.modelCovariance;
+        kalmanGain = variance / (variance + parameters.dataCovariance);
+        state += kalmanGain * (updateProjection - state);
+        variance *= (1.0 - kalmanGain);
+        previousState = state;
+        previousVariance = variance;
+    }
+
+    public double getState() {
+        return state;
+    }
+
+    public void debug(Telemetry telemetry) {
+        telemetry.addData("state", state);
+        telemetry.addData("variance", variance);
+        telemetry.addData("Kalman gain", kalmanGain);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/KalmanFilterParameters.java

@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.util;
+
+import kotlin.jvm.JvmField;
+
+/**
+ * This is the KalmanFilterParameters class. This class handles holding parameters Kalman filters.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 7/17/2024
+ */
+public class KalmanFilterParameters {
+    @JvmField public double modelCovariance;
+    @JvmField public double dataCovariance;
+
+    /**
+     * This creates a new KalmanFilterParameters with a specified model and data covariance.
+     *
+     * @param modelCovariance the covariance of the model.
+     * @param dataCovariance the covariance of the data.
+     */
+    public KalmanFilterParameters(double modelCovariance, double dataCovariance) {
+        this.modelCovariance = modelCovariance;
+        this.dataCovariance = dataCovariance;
+    }
+}