diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java index f2cfcfda..a6ed2384 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/DriveVectorScaler.java @@ -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 @@ -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]; @@ -65,8 +66,8 @@ 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 { @@ -74,7 +75,7 @@ public double[] getDrivePowers(Vector correctivePower, Vector headingPower, Vect 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 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)); @@ -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; @@ -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 @@ -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; } } 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 23885ca8..8753f59c 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 @@ -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 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); } /** @@ -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); } @@ -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); } @@ -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))); } @@ -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); @@ -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; }