SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

work mostly done on kalman filter

Browse Source
This commit is contained in:
brotherhobo
2024-07-20 00:56:24 -04:00
parent 67f56a9931
commit 7b8adf25bf
2 changed files with 16 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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.smallHeadingPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; 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.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.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@ -126,10 +124,9 @@ public class Follower {
private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients); private FilteredPIDFController largeDrivePIDF = new FilteredPIDFController(FollowerConstants.largeDrivePIDFCoefficients);
private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters); private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
private long[] driveErrorTimes;
private double[] driveErrors; private double[] driveErrors;
private double rawDriverError; private double rawDriveError;
private double previousRawDriverError; private double previousRawDriveError;
public static boolean drawOnDashboard = true; public static boolean drawOnDashboard = true;
public static boolean useTranslational = true; public static boolean useTranslational = true;
@ -589,14 +586,13 @@ public class Follower {
correctiveVector = new Vector(); correctiveVector = new Vector();
driveError = 0; driveError = 0;
headingError = 0; headingError = 0;
rawDriverError = 0; rawDriveError = 0;
previousRawDriverError = 0; previousRawDriveError = 0;
driveErrors = new double[3]; driveErrors = new double[2];
driveErrorTimes = new long[3]; for (int i = 0; i < driveErrors.length; i++) {
for (int i = 0; i < driveErrorTimes.length; i++) { driveErrors[i] = 0;
driveErrorTimes[i] = System.currentTimeMillis();
} }
driveKalmanFilter.reset(0, tunedDriveErrorVariance, tunedDriveErrorKalmanGain); driveKalmanFilter.reset();
for (int i = 0; i < motors.size(); i++) { for (int i = 0; i < motors.size(); i++) {
motors.get(i).setPower(0); motors.get(i).setPower(0);
@ -682,24 +678,17 @@ public class Follower {
Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta()); Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta());
Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError);
previousRawDriverError = rawDriverError; previousRawDriveError = rawDriveError;
rawDriverError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
double previousErrorVelocity = (driveErrors[1] - driveErrors[0]) / ((driveErrorTimes[1] - driveErrorTimes[0]) / 1000.0); double projection = 2 * driveErrors[2] - driveErrors[1];
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(rawDriveError - previousRawDriveError, projection);
driveKalmanFilter.update(rawDriverError - previousRawDriverError, projection);
for (int i = 0; i < driveErrors.length - 1; i++) { for (int i = 0; i < driveErrors.length - 1; i++) {
driveErrors[i] = driveErrors[i + 1]; driveErrors[i] = driveErrors[i + 1];
driveErrorTimes[i] = driveErrorTimes[i + 1];
} }
driveErrors[2] = driveKalmanFilter.getState(); driveErrors[2] = driveKalmanFilter.getState();
driveErrorTimes[2] = System.currentTimeMillis();
return driveKalmanFilter.getState(); return driveKalmanFilter.getState();
} }

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

@ -94,7 +94,7 @@ public class FollowerConstants {
0.025, 0.025,
0, 0,
0.00001, 0.00001,
0.01, 0.6,
0); 0);
// Feed forward constant added on to the large drive PIDF // Feed forward constant added on to the large drive PIDF
@ -108,7 +108,7 @@ public class FollowerConstants {
0.02, 0.02,
0, 0,
0.000005, 0.000005,
0.01, 0.6,
0); 0);
// Feed forward constant added on to the small drive PIDF // 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 // Kalman filter parameters for the drive error Kalman filter
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters( public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
0.4, 6,
0.1); 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 // Mass of robot in kilograms
public static double mass = 10.65942; public static double mass = 10.65942;