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

Merge pull request #10 from TBHGodPro/master

Browse Source 
Multiple usability patches
This commit is contained in:
Anyi Lin
2024-10-28 19:56:48 -04:00
committed by GitHub
parent 85b8daed3d 2530445d0d
commit 04c02fb36e
11 changed files with 187 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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];

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java
View File

@ -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()) + ")";
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java
View File

@ -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.
*
* <p>
* 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));
}
/**
@ -175,8 +175,8 @@ public class MathFunctions {
/**
* This rotates the given pose by the given theta,
*
* @param pose the Pose to rotate.
* @param theta the angle to rotate by.
* @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.
*/
@ -196,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);
}
/**
@ -229,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());
}
/**
@ -243,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());
}
}
@ -256,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;
}
@ -270,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;
}
@ -282,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();
}
/**
@ -294,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();
}
/**
@ -302,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.
*/

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java
View File

@ -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<Point> 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;
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java
View File

@ -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() + ")";
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -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(

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java
View File

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

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardZeroPowerAccelerationTuner.java
View File

@ -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<Double> 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();

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java
View File

@ -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<Double> 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();

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/StrafeVelocityTuner.java
View File

@ -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<Double> 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);
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java
View File

@ -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);