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 fe9d153..5089eb4 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 @@ -1,19 +1,19 @@ package org.firstinspires.ftc.teamcode.pedroPathing.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward; +import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID; @@ -365,6 +365,25 @@ public class Follower { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); } + /** + * This holds a Point. + * + * @param point the Point to stay at. + * @param heading the heading to face. + */ + public void holdPoint(Point point, double heading) { + holdPoint(new BezierPoint(point), heading); + } + + /** + * This holds a Point. + * + * @param pose the Point (as a Pose) to stay at. + */ + public void holdPoint(Pose pose) { + holdPoint(new Point(pose), pose.getHeading()); + } + /** * This follows a Path. * This also makes the Follower hold the last Point on the Path. @@ -426,15 +445,22 @@ public class Follower { } /** - * This calls an update to the PoseUpdater, which updates the robot's current position estimate. - * This also updates all the Follower's PIDFs, which updates the motor powers. + * Calls an update to the PoseUpdater, which updates the robot's current position estimate. */ - public void update() { + public void updatePose() { poseUpdater.update(); if (drawOnDashboard) { dashboardPoseTracker.update(); } + } + + /** + * This calls an update to the PoseUpdater, which updates the robot's current position estimate. + * This also updates all the Follower's PIDFs, which updates the motor powers. + */ + public void update() { + updatePose(); if (!teleopDrive) { if (currentPath != null) { @@ -515,7 +541,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) { setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true); @@ -528,7 +554,7 @@ public class Follower { * movement, this is the x-axis. * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric * movement, this is the y-axis. - * @param heading determines the heading vector for the robot in teleop. + * @param heading determines the heading vector for the robot in teleop. * @param robotCentric sets if the movement will be field or robot centric */ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) { @@ -719,7 +745,7 @@ public class Follower { Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError); previousRawDriveError = rawDriveError; - rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); + rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector())); double projection = 2 * driveErrors[1] - driveErrors[0]; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java index d5ac720..d784c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -44,7 +46,7 @@ public class Pose { * This creates a new Pose with no inputs and 0 for all values. */ public Pose() { - this(0,0,0); + this(0, 0, 0); } /** @@ -208,4 +210,10 @@ public class Pose { public Pose copy() { return new Pose(getX(), getY(), getHeading()); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")"; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index e4e1f34..a700e3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -21,18 +21,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 7/20/2024 @@ -108,7 +108,10 @@ public class OTOSLocalizer extends Localizer { */ @Override public Pose getPose() { - return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h)); + SparkFunOTOS.Pose2D rawPose = otos.getPosition(); + Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** @@ -219,4 +222,4 @@ public class OTOSLocalizer extends Localizer { */ public void resetIMU() { } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index 151311b..232df9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || | - * | || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * With the pinpoint your readings will be used in mm * to use inches ensure to divide your mm value by 25.4 * @author Logan Nash @@ -96,8 +96,10 @@ public class PinpointLocalizer extends Localizer { */ @Override public Pose getPose() { - Pose2D pose = odo.getPosition(); - return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); + Pose2D rawPose = odo.getPosition(); + Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); + + return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index 1e9fd7b..bd3c598 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Logan Nash * @author Anyi Lin - 10158 Scott's Bots @@ -313,4 +313,4 @@ public class ThreeWheelIMULocalizer extends Localizer { public void resetIMU() { imu.resetYaw(); } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index 6814b86..4368dc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) + * forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || || | + * left (y positive) <--- | || || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 62415d6..50d7ec2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; * * forward on robot is the x positive direction * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) +* forward (x positive) + * △ + * | + * | + * /--------------\ + * | | + * | | + * | || | + * left (y positive) <--- | || | + * | ____ | + * | ---- | + * \--------------/ * * @author Anyi Lin - 10158 Scott's Bots * @version 1.0, 4/2/2024 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index 0edfbfd..bd4e393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -27,16 +27,16 @@ public class MathFunctions { double denom = 1; // this multiplies up the numerator of the nCr function - for (int i = n; i > n-r; i--) { + for (int i = n; i > n - r; i--) { num *= i; } // this multiplies up the denominator of the nCr function - for (int i = 1; i <=r; i++) { + for (int i = 1; i <= r; i++) { denom *= i; } - return num/denom; + return num / denom; } /** @@ -67,7 +67,7 @@ public class MathFunctions { /** * This normalizes an angle to be between 0 and 2 pi radians, inclusive. - * + *

* IMPORTANT NOTE: This method operates in radians. * * @param angleRadians the angle to be normalized. @@ -75,8 +75,8 @@ public class MathFunctions { */ public static double normalizeAngle(double angleRadians) { double angle = angleRadians; - while (angle<0) angle += 2*Math.PI; - while (angle>2*Math.PI) angle -= 2*Math.PI; + while (angle < 0) angle += 2 * Math.PI; + while (angle > 2 * Math.PI) angle -= 2 * Math.PI; return angle; } @@ -88,7 +88,7 @@ public class MathFunctions { * @return returns the smallest angle. */ public static double getSmallestAngleDifference(double one, double two) { - return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one)); + return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one)); } /** @@ -98,7 +98,7 @@ public class MathFunctions { * @return returns the turn direction. */ public static double getTurnDirection(double startHeading, double endHeading) { - if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) { + if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) { return 1; // counter clock wise } return -1; // clock wise @@ -112,7 +112,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose pose, Point point) { - return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); + return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2)); } /** @@ -123,7 +123,7 @@ public class MathFunctions { * @return returns the distance between the two. */ public static double distance(Pose one, Pose two) { - return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); + return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2)); } /** @@ -172,6 +172,22 @@ public class MathFunctions { return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); } + /** + * This rotates the given pose by the given theta, + * + * @param pose the Pose to rotate. + * @param theta the angle to rotate by. + * @param rotateHeading whether to adjust the Pose heading too. + * @return the rotated Pose. + */ + public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) { + double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta); + double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta); + double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading(); + + return new Pose(x, y, heading); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * @@ -180,7 +196,7 @@ public class MathFunctions { * @return returns the scaled Point. */ public static Point scalarMultiplyPoint(Point point, double scalar) { - return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN); + return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN); } /** @@ -213,7 +229,7 @@ public class MathFunctions { * @return returns the scaled Vector. */ public static Vector scalarMultiplyVector(Vector vector, double scalar) { - return new Vector(vector.getMagnitude()*scalar, vector.getTheta()); + return new Vector(vector.getMagnitude() * scalar, vector.getTheta()); } /** @@ -227,7 +243,7 @@ public class MathFunctions { if (vector.getMagnitude() == 0) { return new Vector(0.0, vector.getTheta()); } else { - return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta()); + return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta()); } } @@ -240,7 +256,7 @@ public class MathFunctions { */ public static Vector addVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent()); return returnVector; } @@ -254,7 +270,7 @@ public class MathFunctions { */ public static Vector subtractVectors(Vector one, Vector two) { Vector returnVector = new Vector(); - returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent()); + returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent()); return returnVector; } @@ -266,7 +282,7 @@ public class MathFunctions { * @return returns the dot product of the two Vectors. */ public static double dotProduct(Vector one, Vector two) { - return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent(); + return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent(); } /** @@ -278,7 +294,7 @@ public class MathFunctions { * @return returns the cross product of the two Vectors. */ public static double crossProduct(Vector one, Vector two) { - return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent(); + return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent(); } /** @@ -286,7 +302,7 @@ public class MathFunctions { * specified accuracy amount. * * @param one first number specified. - * @param two second number specified. + * @param two Second number specified. * @param accuracy the level of accuracy specified. * @return returns if the two numbers are within the specified accuracy of each other. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java index 5bfff7d..f044ce7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -51,16 +51,47 @@ public class PathBuilder { return this; } + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(Point... controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierCurve to the PathBuilder. + * + * @param controlPoints This is the specified control points that define the BezierCurve. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierCurve(ArrayList controlPoints) { + return addPath(new BezierCurve(controlPoints)); + } + + /** + * This adds a default Path defined by a specified BezierLine to the PathBuilder. + * + * @param startPoint start point of the line. + * @param endPoint end point of the line. + * @return This returns itself with the updated data. + */ + public PathBuilder addBezierLine(Point startPoint, Point endPoint) { + return addPath(new BezierLine(startPoint, endPoint)); + } + /** * This sets a linear heading interpolation on the last Path added to the PathBuilder. * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading); return this; } @@ -69,13 +100,13 @@ public class PathBuilder { * * @param startHeading The start of the linear heading interpolation. * @param endHeading The end of the linear heading interpolation. - * This will be reached at the end of the Path if no end time is specified. + * This will be reached at the end of the Path if no end time is specified. * @param endTime The end time on the Path that the linear heading interpolation will end. - * This value goes from [0, 1] since Bezier curves are parametric functions. + * This value goes from [0, 1] since Bezier curves are parametric functions. * @return This returns itself with the updated data. */ public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) { - this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); + this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime); return this; } @@ -86,7 +117,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setConstantHeadingInterpolation(double setHeading) { - this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading); + this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading); return this; } @@ -94,20 +125,21 @@ public class PathBuilder { * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder. * * @param set This sets the heading to reversed tangent following if this parameter is true. - * Conversely, this sets a tangent following if this parameter is false. + * Conversely, this sets a tangent following if this parameter is false. * @return This returns itself with the updated data. */ public PathBuilder setReversed(boolean set) { - this.paths.get(paths.size()-1).setReversed(set); + this.paths.get(paths.size() - 1).setReversed(set); return this; } + /** * This sets the heading interpolation to tangential on the last Path added to the PathBuilder. * There really shouldn't be a reason to use this since the default heading interpolation is * tangential but it's here. */ public PathBuilder setTangentHeadingInterpolation() { - this.paths.get(paths.size()-1).setTangentHeadingInterpolation(); + this.paths.get(paths.size() - 1).setTangentHeadingInterpolation(); return this; } @@ -118,7 +150,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setZeroPowerAccelerationMultiplier(double set) { - this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set); + this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set); return this; } @@ -129,7 +161,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndVelocityConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set); + this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set); return this; } @@ -140,7 +172,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTranslationalConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set); return this; } @@ -151,7 +183,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndHeadingConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set); + this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set); return this; } @@ -162,7 +194,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTValueConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTValueConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set); return this; } @@ -173,7 +205,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder setPathEndTimeoutConstraint(double set) { - this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set); + this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set); return this; } @@ -182,12 +214,12 @@ public class PathBuilder { * This callback is set to run at a specified number of milliseconds after the start of the path. * * @param time This sets the number of milliseconds of wait between the start of the Path and - * the calling of the callback. + * the calling of the callback. * @param runnable This sets the code for the callback to run. Use lambda statements for this. * @return This returns itself with the updated data. */ public PathBuilder addTemporalCallback(double time, Runnable runnable) { - this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1)); + this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1)); return this; } @@ -200,7 +232,7 @@ public class PathBuilder { * @return This returns itself with the updated data. */ public PathBuilder addParametricCallback(double t, Runnable runnable) { - this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1)); + this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1)); return this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index 0da8df8..95a5f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; +import androidx.annotation.NonNull; + import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** @@ -31,13 +33,13 @@ public class Point { * This creates a new Point with coordinate inputs and a specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system the coordinate inputs are in. */ public Point(double rOrX, double thetaOrY, int identifier) { @@ -53,17 +55,27 @@ public class Point { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } + /** + * This creates a new Point from a X and Y value. + * + * @param setX the X value. + * @param setY the Y value. + */ + public Point(double setX, double setY) { + setCoordinates(setX, setY, CARTESIAN); + } + /** * This sets the coordinates of the Point using the specified coordinate system. * * @param rOrX Depending on the coordinate system specified, this is either the r or x value. - * In polar coordinates, the r value is the distance from the origin. - * In Cartesian coordinates, the x value is the distance left/right from the origin. + * In polar coordinates, the r value is the distance from the origin. + * In Cartesian coordinates, the x value is the distance left/right from the origin. * @param thetaOrY Depending on the coordinate system specified, this is either the theta or - * y value. - * In polar coordinates, the theta value is the angle from the positive x-axis. - * Increasing theta moves in the counter-clockwise direction. - * In Cartesian coordinates, the y value is the distance up/down from the origin. + * y value. + * In polar coordinates, the theta value is the angle from the positive x-axis. + * Increasing theta moves in the counter-clockwise direction. + * In Cartesian coordinates, the y value is the distance up/down from the origin. * @param identifier this specifies what coordinate system to use when setting values. */ public void setCoordinates(double rOrX, double thetaOrY, int identifier) { @@ -78,9 +90,9 @@ public class Point { theta = setOtherCoordinates[1]; break; default: - if (rOrX<0) { + if (rOrX < 0) { r = -rOrX; - theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI); + theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI); } else { r = rOrX; theta = MathFunctions.normalizeAngle(thetaOrY); @@ -99,7 +111,7 @@ public class Point { * @return returns the distance between the two Points. */ public double distanceFrom(Point otherPoint) { - return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2)); + return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2)); } /** @@ -110,7 +122,7 @@ public class Point { * @return this returns the x and y values, in that order, in an Array of doubles. */ public static double[] polarToCartesian(double r, double theta) { - return new double[] {r * Math.cos(theta), r * Math.sin(theta)}; + return new double[]{r * Math.cos(theta), r * Math.sin(theta)}; } /** @@ -123,17 +135,17 @@ public class Point { public static double[] cartesianToPolar(double x, double y) { if (x == 0) { if (y > 0) { - return new double[] {Math.abs(y), Math.PI/2}; + return new double[]{Math.abs(y), Math.PI / 2}; } else { - return new double[] {Math.abs(y), (3 * Math.PI) / 2}; + return new double[]{Math.abs(y), (3 * Math.PI) / 2}; } } - double r = Math.sqrt(x*x+y*y); - if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)}; + double r = Math.sqrt(x * x + y * y); + if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)}; if (y > 0) { return new double[]{r, Math.atan(y / x)}; } else { - return new double[]{r, (2*Math.PI) + Math.atan(y / x)}; + return new double[]{r, (2 * Math.PI) + Math.atan(y / x)}; } } @@ -182,4 +194,10 @@ public class Point { public Point copy() { return new Point(getX(), getY(), CARTESIAN); } + + @NonNull + @Override + public String toString() { + return "(" + getX() + ", " + getY() + ")"; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index af72609..bd932cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; import com.acmerobotics.dashboard.config.Config; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; -import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; +import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients; import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; /** @@ -33,7 +33,7 @@ public class FollowerConstants { private static double xMovement = 81.34056; private static double yMovement = 65.43028; private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); - public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); + public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1])); // Translational PIDF coefficients (don't use integral) @@ -182,7 +182,7 @@ public class FollowerConstants { // the limit at which the heading PIDF switches between the main and secondary heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; + public static double headingPIDFSwitch = Math.PI / 20; // Secondary heading error PIDF coefficients public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java index e97f78a..ad07d88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning") public class ForwardVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -122,6 +122,10 @@ public class ForwardVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java index 9454e13..f7393bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class ForwardZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,6 +120,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -149,7 +153,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("forward zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 505245d..87b1978 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode { private ArrayList accelerations = new ArrayList<>(); @@ -120,11 +120,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } poseUpdater.update(); - Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2); + Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2); if (!end) { if (!stopping) { if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) { @@ -149,7 +153,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { for (Double acceleration : accelerations) { average += acceleration; } - average /= (double)accelerations.size(); + average /= (double) accelerations.size(); telemetryA.addData("lateral zero power acceleration (deceleration):", average); telemetryA.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java index acd90c0..ad6d22b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java @@ -40,7 +40,7 @@ import java.util.List; * @version 1.0, 3/13/2024 */ @Config -@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") +@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning") public class StrafeVelocityTuner extends OpMode { private ArrayList velocities = new ArrayList<>(); @@ -120,6 +120,10 @@ public class StrafeVelocityTuner extends OpMode { @Override public void loop() { if (gamepad1.cross || gamepad1.a) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motor.setPower(0); + } requestOpModeStop(); } @@ -132,7 +136,7 @@ public class StrafeVelocityTuner extends OpMode { motor.setPower(0); } } else { - double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2))); + double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2))); velocities.add(currentVelocity); velocities.remove(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java index 9a2d956..9af9662 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; * @version 1.0, 4/22/2024 */ public class Drawing { + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * * @param follower */ public static void drawDebug(Follower follower) { @@ -35,6 +38,7 @@ public class Drawing { } drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50"); drawRobot(follower.getPose(), "#4CAF50"); + sendPacket(); } @@ -114,12 +118,10 @@ public class Drawing { * @param t the Point to draw at */ public static void drawRobotOnCanvas(Canvas c, Point t) { - final double ROBOT_RADIUS = 9; - c.setStrokeWidth(1); c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); - Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta()); Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); Vector p2 = MathFunctions.addVectors(p1, halfv); c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); @@ -133,8 +135,6 @@ public class Drawing { * @param t the Pose to draw at */ public static void drawRobotOnCanvas(Canvas c, Pose t) { - final double ROBOT_RADIUS = 9; - c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); Vector v = t.getHeadingVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);