From 7b8adf25bfd574995201e94abca886edfbe9625e Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 20 Jul 2024 00:56:24 -0400 Subject: [PATCH] work mostly done on kalman filter --- .../pedroPathing/follower/Follower.java | 35 +++++++------------ .../tuning/FollowerConstants.java | 12 +++---- 2 files changed, 16 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 03eee3c..383b7aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -11,8 +11,6 @@ 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; @@ -126,10 +124,9 @@ public class Follower { 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; + private double rawDriveError; + private double previousRawDriveError; public static boolean drawOnDashboard = true; public static boolean useTranslational = true; @@ -589,14 +586,13 @@ 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(); + rawDriveError = 0; + previousRawDriveError = 0; + driveErrors = new double[2]; + for (int i = 0; i < driveErrors.length; i++) { + driveErrors[i] = 0; } - driveKalmanFilter.reset(0, tunedDriveErrorVariance, tunedDriveErrorKalmanGain); + driveKalmanFilter.reset(); for (int i = 0; i < motors.size(); i++) { motors.get(i).setPower(0); @@ -682,24 +678,17 @@ public class Follower { Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); - previousRawDriverError = rawDriverError; - rawDriverError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + previousRawDriveError = rawDriveError; + rawDriveError = 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 = 2 * driveErrors[2] - driveErrors[1]; - double projection = errorVelocity * time + 0.5 * errorAcceleration * Math.pow(time, 2); - - driveKalmanFilter.update(rawDriverError - previousRawDriverError, projection); + driveKalmanFilter.update(rawDriveError - previousRawDriveError, 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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 60dcd9a..4569d77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -94,7 +94,7 @@ public class FollowerConstants { 0.025, 0, 0.00001, - 0.01, + 0.6, 0); // Feed forward constant added on to the large drive PIDF @@ -108,7 +108,7 @@ public class FollowerConstants { 0.02, 0, 0.000005, - 0.01, + 0.6, 0); // Feed forward constant added on to the small drive PIDF @@ -116,12 +116,8 @@ public class FollowerConstants { // 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; + 6, + 1); // Mass of robot in kilograms public static double mass = 10.65942;