Skip to content

Commit

Permalink
Merge pull request #10 from TBHGodPro/master
Browse files Browse the repository at this point in the history 
Multiple usability patches
  • Loading branch information
@AnyiLin
AnyiLin authored Oct 28, 2024
2 parents 85b8dae + 2530445 commit 04c02fb
Show file tree
Hide file tree
Showing 11 changed files with 187 additions and 87 deletions.
46 changes: 36 additions & 10 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
@@ -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;
Expand Down Expand Up @@ -365,6 +365,25 @@ public void holdPoint(BezierPoint point, double heading) {
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.
Expand Down Expand Up @@ -426,15 +445,22 @@ public void startTeleopDrive() {
}

/**
* 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) {
Expand Down Expand Up @@ -515,7 +541,7 @@ public void update() {
* 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);
Expand All @@ -528,7 +554,7 @@ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, d
* 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) {
Expand Down Expand Up @@ -719,7 +745,7 @@ public double getDriveVelocityError() {
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];

Expand Down
10 changes: 9 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java
View file
Original file line number Diff line number Diff line change
@@ -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;

Expand Down Expand Up @@ -44,7 +46,7 @@ public Pose(double setX, double setY) {
* This creates a new Pose with no inputs and 0 for all values.
*/
public Pose() {
this(0,0,0);
this(0, 0, 0);
}

/**
Expand Down Expand Up @@ -208,4 +210,10 @@ public boolean roughlyEquals(Pose pose) {
public Pose copy() {
return new Pose(getX(), getY(), getHeading());
}

@NonNull
@Override
public String toString() {
return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")";
}
}
40 changes: 20 additions & 20 deletions ...c/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,16 +27,16 @@ public static double nCr(int n, int r) {
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;
}

/**
Expand Down Expand Up @@ -67,16 +67,16 @@ public static double clamp(double num, double lower, double upper) {

/**
* 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.
* @return returns the normalized angle.
*/
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;
}

Expand All @@ -88,7 +88,7 @@ public static double normalizeAngle(double angleRadians) {
* @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));
}

/**
Expand All @@ -98,7 +98,7 @@ public static double getSmallestAngleDifference(double one, double two) {
* @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
Expand All @@ -112,7 +112,7 @@ public static double getTurnDirection(double startHeading, double endHeading) {
* @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));
}

/**
Expand All @@ -123,7 +123,7 @@ public static double distance(Pose pose, Point point) {
* @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));
}

/**
Expand Down Expand Up @@ -175,8 +175,8 @@ public static Pose subtractPoses(Pose one, Pose two) {
/**
* 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.
*/
Expand All @@ -196,7 +196,7 @@ public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) {
* @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);
}

/**
Expand Down Expand Up @@ -229,7 +229,7 @@ public static Vector copyVector(Vector vector) {
* @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());
}

/**
Expand All @@ -243,7 +243,7 @@ public static Vector normalizeVector(Vector vector) {
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());
}
}

Expand All @@ -256,7 +256,7 @@ public static Vector normalizeVector(Vector vector) {
*/
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;
}

Expand All @@ -270,7 +270,7 @@ public static Vector addVectors(Vector one, Vector two) {
*/
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;
}

Expand All @@ -282,7 +282,7 @@ public static Vector subtractVectors(Vector one, Vector two) {
* @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();
}

/**
Expand All @@ -294,15 +294,15 @@ public static double dotProduct(Vector one, Vector two) {
* @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();
}

/**
* This returns whether a specified value is within a second specified value by plus/minus a
* 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.
*/
Expand Down
Loading

0 comments on commit 04c02fb

Please sign in to comment.