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

Multiple usability patches

Browse Source
This commit is contained in:
TBHGodPro
2024-10-28 17:39:03 -05:00
parent 85b8daed3d
commit 2530445d0d
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);