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.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();
}

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,
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;