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

makes limiting the max power better

Browse Source
This commit is contained in:
Anyi Lin
2024-12-15 18:14:56 -05:00
parent a2d896b67c
commit c04de0428e
2 changed files with 42 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java
View File

@ -15,6 +15,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
public class DriveVectorScaler {
// This is ordered left front, left back, right front, right back. These are also normalized.
private Vector[] mecanumVectors;
private double maxPowerScaling = 1;
/**
* This creates a new DriveVectorScaler, which takes in various movement vectors and outputs
@ -52,9 +53,9 @@ public class DriveVectorScaler {
*/
public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vector pathingPower, double robotHeading) {
// clamps down the magnitudes of the input vectors
if (correctivePower.getMagnitude() > 1) correctivePower.setMagnitude(1);
if (headingPower.getMagnitude() > 1) headingPower.setMagnitude(1);
if (pathingPower.getMagnitude() > 1) pathingPower.setMagnitude(1);
if (correctivePower.getMagnitude() > maxPowerScaling) correctivePower.setMagnitude(maxPowerScaling);
if (headingPower.getMagnitude() > maxPowerScaling) headingPower.setMagnitude(maxPowerScaling);
if (pathingPower.getMagnitude() > maxPowerScaling) pathingPower.setMagnitude(maxPowerScaling);
// the powers for the wheel vectors
double [] wheelPowers = new double[4];
@ -65,8 +66,8 @@ public class DriveVectorScaler {
// this contains the pathing vectors, one for each side (heading control requires 2)
Vector[] truePathingVectors = new Vector[2];
if (correctivePower.getMagnitude() == 1) {
// checks for corrective power equal to 1 in magnitude. if equal to one, then set pathing power to that
if (correctivePower.getMagnitude() == maxPowerScaling) {
// checks for corrective power equal to max power scaling in magnitude. if equal, then set pathing power to that
truePathingVectors[0] = MathFunctions.copyVector(correctivePower);
truePathingVectors[1] = MathFunctions.copyVector(correctivePower);
} else {
@ -74,7 +75,7 @@ public class DriveVectorScaler {
Vector leftSideVector = MathFunctions.subtractVectors(correctivePower, headingPower);
Vector rightSideVector = MathFunctions.addVectors(correctivePower, headingPower);
if (leftSideVector.getMagnitude() > 1 || rightSideVector.getMagnitude() > 1) {
if (leftSideVector.getMagnitude() > maxPowerScaling || rightSideVector.getMagnitude() > maxPowerScaling) {
//if the combined corrective and heading power is greater than 1, then scale down heading power
double headingScalingFactor = Math.min(findNormalizingScaling(correctivePower, headingPower), findNormalizingScaling(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, -1)));
truePathingVectors[0] = MathFunctions.subtractVectors(correctivePower, MathFunctions.scalarMultiplyVector(headingPower, headingScalingFactor));
@ -84,7 +85,7 @@ public class DriveVectorScaler {
Vector leftSideVectorWithPathing = MathFunctions.addVectors(leftSideVector, pathingPower);
Vector rightSideVectorWithPathing = MathFunctions.addVectors(rightSideVector, pathingPower);
if (leftSideVectorWithPathing.getMagnitude() > 1 || rightSideVectorWithPathing.getMagnitude() > 1) {
if (leftSideVectorWithPathing.getMagnitude() > maxPowerScaling || rightSideVectorWithPathing.getMagnitude() > maxPowerScaling) {
// too much power now, so we scale down the pathing vector
double pathingScalingFactor = Math.min(findNormalizingScaling(leftSideVector, pathingPower), findNormalizingScaling(rightSideVector, pathingPower));
truePathingVectors[0] = MathFunctions.addVectors(leftSideVector, MathFunctions.scalarMultiplyVector(pathingPower, pathingScalingFactor));
@ -113,7 +114,7 @@ public class DriveVectorScaler {
wheelPowers[3] = (mecanumVectorsCopy[2].getXComponent()*truePathingVectors[1].getYComponent() - truePathingVectors[1].getXComponent()*mecanumVectorsCopy[2].getYComponent()) / (mecanumVectorsCopy[2].getXComponent()*mecanumVectorsCopy[3].getYComponent() - mecanumVectorsCopy[3].getXComponent()*mecanumVectorsCopy[2].getYComponent());
double wheelPowerMax = Math.max(Math.max(Math.abs(wheelPowers[0]), Math.abs(wheelPowers[1])), Math.max(Math.abs(wheelPowers[2]), Math.abs(wheelPowers[3])));
if (wheelPowerMax > 1) {
if (wheelPowerMax > maxPowerScaling) {
wheelPowers[0] /= wheelPowerMax;
wheelPowers[1] /= wheelPowerMax;
wheelPowers[2] /= wheelPowerMax;
@ -126,12 +127,12 @@ public class DriveVectorScaler {
/**
* This takes in two Vectors, one static and one variable, and returns the scaling factor that,
* when multiplied to the variable Vector, results in magnitude of the sum of the static Vector
* and the scaled variable Vector being 1.
* and the scaled variable Vector being the max power scaling.
*
* IMPORTANT NOTE: I did not intend for this to be used for anything other than the method above
* this one in this class, so there will be errors if you input Vectors of length greater than 1,
* this one in this class, so there will be errors if you input Vectors of length greater than maxPowerScaling,
* and it will scale up the variable Vector if the magnitude of the sum of the two input Vectors
* isn't greater than 1. So, just don't use this elsewhere. There's gotta be a better way to do
* isn't greater than maxPowerScaling. So, just don't use this elsewhere. There's gotta be a better way to do
* whatever you're trying to do.
*
* I know that this is used outside of this class, however, I created this method so I get to
@ -140,13 +141,32 @@ public class DriveVectorScaler {
*
* @param staticVector the Vector that is held constant.
* @param variableVector the Vector getting scaled to make the sum of the input Vectors have a
* magnitude of 1.
* magnitude of maxPowerScaling.
* @return returns the scaling factor for the variable Vector.
*/
public double findNormalizingScaling(Vector staticVector, Vector variableVector) {
double a = Math.pow(variableVector.getXComponent(), 2) + Math.pow(variableVector.getYComponent(), 2);
double b = staticVector.getXComponent() * variableVector.getXComponent() + staticVector.getYComponent() * variableVector.getYComponent();
double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - 1.0;
double c = Math.pow(staticVector.getXComponent(), 2) + Math.pow(staticVector.getYComponent(), 2) - Math.pow(maxPowerScaling, 2);
return (-b + Math.sqrt(Math.pow(b, 2) - a*c))/(a);
}
/**
* Sets the maximum power that can be used by the drive vector scaler. Clamped between 0 and 1.
*
* @param maxPowerScaling setting the max power scaling
*/
public void setMaxPowerScaling(double maxPowerScaling) {
this.maxPowerScaling = MathFunctions.clamp(maxPowerScaling, 0, 1);
}
/**
* Gets the maximum power that can be used by the drive vector scaler. Ranges between 0 and 1.
*
* @return returns the max power scaling
*/
public double getMaxPowerScaling() {
return maxPowerScaling;
}
}

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

@ -93,7 +93,6 @@ public class Follower {
private boolean holdPositionAtEnd;
private boolean teleopDrive;
private double maxPower = 1;
private double previousSecondaryTranslationalIntegral;
private double previousTranslationalIntegral;
private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
@ -194,18 +193,7 @@ public class Follower {
* @param set This caps the motor power from [0, 1].
*/
public void setMaxPower(double set) {
maxPower = MathFunctions.clamp(set, 0, 1);
}
/**
* This handles the limiting of the drive powers array to the max power.
*/
public void limitDrivePowers() {
for (int i = 0; i < drivePowers.length; i++) {
if (Math.abs(drivePowers[i]) > maxPower) {
drivePowers[i] = maxPower * MathFunctions.getSign(drivePowers[i]);
}
}
driveVectorScaler.setMaxPowerScaling(set);
}
/**
@ -693,19 +681,19 @@ public class Follower {
public Vector getDriveVector() {
if (!useDrive) return new Vector();
if (followingPathChain && chainIndex < currentPathChain.size() - 1) {
return new Vector(1, currentPath.getClosestPointTangentVector().getTheta());
return new Vector(driveVectorScaler.getMaxPowerScaling(), currentPath.getClosestPointTangentVector().getTheta());
}
driveError = getDriveVelocityError();
if (Math.abs(driveError) < drivePIDFSwitch && useSecondaryDrivePID) {
secondaryDrivePIDF.updateError(driveError);
driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
driveVector = new Vector(MathFunctions.clamp(secondaryDrivePIDF.runPIDF() + secondaryDrivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta());
return MathFunctions.copyVector(driveVector);
}
drivePIDF.updateError(driveError);
driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -1, 1), currentPath.getClosestPointTangentVector().getTheta());
driveVector = new Vector(MathFunctions.clamp(drivePIDF.runPIDF() + drivePIDFFeedForward * MathFunctions.getSign(driveError), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta());
return MathFunctions.copyVector(driveVector);
}
@ -774,11 +762,11 @@ public class Follower {
headingError = MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()) * MathFunctions.getSmallestAngleDifference(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal());
if (Math.abs(headingError) < headingPIDFSwitch && useSecondaryHeadingPID) {
secondaryHeadingPIDF.updateError(headingError);
headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
headingVector = new Vector(MathFunctions.clamp(secondaryHeadingPIDF.runPIDF() + secondaryHeadingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading());
return MathFunctions.copyVector(headingVector);
}
headingPIDF.updateError(headingError);
headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -1, 1), poseUpdater.getPose().getHeading());
headingVector = new Vector(MathFunctions.clamp(headingPIDF.runPIDF() + headingPIDFFeedForward * MathFunctions.getTurnDirection(poseUpdater.getPose().getHeading(), currentPath.getClosestPointHeadingGoal()), -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), poseUpdater.getPose().getHeading());
return MathFunctions.copyVector(headingVector);
}
@ -795,7 +783,7 @@ public class Follower {
Vector translational = getTranslationalCorrection();
Vector corrective = MathFunctions.addVectors(centripetal, translational);
if (corrective.getMagnitude() > 1) {
if (corrective.getMagnitude() > driveVectorScaler.getMaxPowerScaling()) {
return MathFunctions.addVectors(centripetal, MathFunctions.scalarMultiplyVector(translational, driveVectorScaler.findNormalizingScaling(centripetal, translational)));
}
@ -844,7 +832,7 @@ public class Follower {
translationalVector = MathFunctions.addVectors(translationalVector, translationalIntegralVector);
}
translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, 1));
translationalVector.setMagnitude(MathFunctions.clamp(translationalVector.getMagnitude(), 0, driveVectorScaler.getMaxPowerScaling()));
this.translationalVector = MathFunctions.copyVector(translationalVector);
@ -883,7 +871,7 @@ public class Follower {
curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
}
if (Double.isNaN(curvature)) return new Vector();
centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
return centripetalVector;
}