Skip to content

Commit

Permalink
makes limiting the max power better
Browse files Browse the repository at this point in the history
  • Loading branch information
@AnyiLin
AnyiLin committed Dec 15, 2024
1 parent a2d896b commit c04de04
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 34 deletions.
46 changes: 33 additions & 13 deletions ...src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
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
Expand Down Expand Up @@ -52,9 +53,9 @@ public DriveVectorScaler(Vector frontLeftVector) {
*/
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];
Expand All @@ -65,16 +66,16 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect
// 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 {
// corrective power did not take up all the power, so add on heading power
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));
Expand All @@ -84,7 +85,7 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect
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));
Expand Down Expand Up @@ -113,7 +114,7 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect
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;
Expand All @@ -126,12 +127,12 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect
/**
* 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
Expand All @@ -140,13 +141,32 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect
*
* @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 changes: 9 additions & 21 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -194,18 +193,7 @@ public void initialize() {
* @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);
}

/**
Expand Down Expand Up @@ -693,19 +681,19 @@ public boolean isBusy() {
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);
}

Expand Down Expand Up @@ -774,11 +762,11 @@ public Vector getHeadingVector() {
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);
}

Expand All @@ -795,7 +783,7 @@ public Vector getCorrectiveVector() {
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)));
}

Expand Down Expand Up @@ -844,7 +832,7 @@ public Vector getTranslationalCorrection() {
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);

Expand Down Expand Up @@ -883,7 +871,7 @@ public Vector getCentripetalForceCorrection() {
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;
}

Expand Down

0 comments on commit c04de04

Please sign in to comment.