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

40
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) {

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

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

@ -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.
@ -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.
*/

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

@ -51,6 +51,37 @@ 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.
*
@ -101,6 +132,7 @@ public class PathBuilder {
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

18
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;
/**
@ -53,6 +55,16 @@ 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.
*
@ -182,4 +194,10 @@ public class Point {
public Point copy() {
return new Point(getX(), getY(), CARTESIAN);
}
@NonNull
@Override
public String toString() {
return "(" + getX() + ", " + getY() + ")";
}
}

4
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;
/**

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

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

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

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

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

@ -120,6 +120,10 @@ 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();
}

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

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

8
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,8 +118,6 @@ 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);
@ -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);