Skip to content

On offer: updated tuning tests that I believe are easier to use #42

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
cwm9cwm9 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

On offer: updated tuning tests that I believe are easier to use #42

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
238 changes: 145 additions & 93 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
import com.bylazar.field.Style;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.ErrorCalculator;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.*;
import com.pedropathing.math.*;
Expand Down Expand Up @@ -741,11 +740,9 @@ public void loop() {
* @version 1.0, 3/12/2024
*/
class TranslationalTuner extends OpMode {
public static double DISTANCE = 40;
private boolean forward = true;
private double impulseDistance = 2;

private Path forwards;
private Path backwards;
private Path testPath;

@Override
public void init() {
Expand All @@ -755,8 +752,9 @@ public void init() {
/** This initializes the Follower and creates the forward and backward Paths. */
@Override
public void init_loop() {
telemetryM.debug("This will activate the translational PIDF(s)");
telemetryM.debug("The robot will try to stay in place while you push it laterally.");
telemetryM.debug("This will activate the translational PIDF(s) only.");
telemetryM.debug("dpad left/right will virtually push the robot off its path.");
telemetryM.debug("dpad up/down will adjust the push distance (default 6 inches) in 0.25 inch increments.");
telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s).");
telemetryM.update(telemetry);
follower.update();
Expand All @@ -767,11 +765,14 @@ public void init_loop() {
public void start() {
follower.deactivateAllPIDFs();
follower.activateTranslational();
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
testPath = new Path(new BezierLine(new Pose(72+6,72), new Pose(72-6,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(2);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}

/** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
Expand All @@ -780,17 +781,21 @@ public void loop() {
follower.update();
draw();

if (!follower.isBusy()) {
if (forward) {
forward = false;
follower.followPath(backwards);
} else {
forward = true;
follower.followPath(forwards);
}
if (gamepad1.dpadRightWasPressed())
{
follower.setPose(new Pose(72, 72+impulseDistance).withHeading(0));
follower.followPath(testPath);
}
if (gamepad1.dpadLeftWasPressed())
{
follower.setPose(new Pose(72, 72-impulseDistance).withHeading(0));
follower.followPath(testPath);
}
if (gamepad1.dpadUpWasPressed()) impulseDistance+=.25;
if (gamepad1.dpadDownWasPressed() && (impulseDistance-.25)>0) impulseDistance-=.25;

telemetryM.debug("Push the robot laterally to test the Translational PIDF(s).");
telemetryM.debug("Push dpad left/right to test the Translational PIDF(s).");
telemetryM.debug("Push dpad up/down to adjust test distance by 0.25 inches: "+impulseDistance+" inches");
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent());
telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent());
Expand All @@ -810,11 +815,9 @@ public void loop() {
* @version 1.0, 3/12/2024
*/
class HeadingTuner extends OpMode {
public static double DISTANCE = 40;
private boolean forward = true;
public static int impulseDegrees = 45;

private Path forwards;
private Path backwards;
private Path testPath;

@Override
public void init() {
Expand All @@ -828,7 +831,8 @@ public void init() {
@Override
public void init_loop() {
telemetryM.debug("This will activate the heading PIDF(s).");
telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it.");
telemetryM.debug("dpad left/right will virtually rotate the robot off its path.");
telemetryM.debug("dpad up/down will adjust the rotation angle (default 45 degrees) in 1 degree increments.");
telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s).");
telemetryM.update(telemetry);
follower.update();
Expand All @@ -839,11 +843,14 @@ public void init_loop() {
public void start() {
follower.deactivateAllPIDFs();
follower.activateHeading();
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
testPath = new Path(new BezierLine(new Pose(72+6,72), new Pose(72-6,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(2);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}

/**
Expand All @@ -855,19 +862,23 @@ public void loop() {
follower.update();
draw();

if (!follower.isBusy()) {
if (forward) {
forward = false;
follower.followPath(backwards);
} else {
forward = true;
follower.followPath(forwards);
}
if (gamepad1.dpadRightWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(Math.toRadians(impulseDegrees)));
follower.followPath(testPath);
}
if (gamepad1.dpadLeftWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(-Math.toRadians(impulseDegrees)));
follower.followPath(testPath);
}
if (gamepad1.dpadUpWasPressed()) impulseDegrees += 1;
if (gamepad1.dpadDownWasPressed() && (impulseDegrees -1 >0)) impulseDegrees -= 1;

telemetryM.debug("Turn the robot manually to test the Heading PIDF(s).");
telemetryM.debug("Push dpad left/right to test the Rotational PIDF(s).");
telemetryM.debug("Push dpad up/down to adjust test rotation by 1 degrees: "+impulseDegrees+" degrees");
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getHeadingError());
telemetryM.addData("Error", Math.toDegrees(follower.errorCalculator.getHeadingError()));
telemetryM.update(telemetry);
}
}
Expand All @@ -882,11 +893,8 @@ public void loop() {
* @version 1.0, 3/12/2024
*/
class DriveTuner extends OpMode {
public static double DISTANCE = 40;
private boolean forward = true;

private PathChain forwards;
private PathChain backwards;
public int impulseDistance = 12;
private Path testPath;

@Override
public void init() {
Expand All @@ -899,9 +907,9 @@ public void init() {
*/
@Override
public void init_loop() {
telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward.");
telemetryM.debug("The robot will go forward and backward continuously along the path.");
telemetryM.debug("Make sure you have enough room.");
telemetryM.debug("This will run the robot in a straight line going inches forward without heading or translational correction.");
telemetryM.debug("Push dpad left/right to drive forward/backward by 12 inches.");
telemetryM.debug("Push dpad up/down to adjust the test distance by 1 inch.");
telemetryM.update(telemetry);
follower.update();
drawOnlyCurrent();
Expand All @@ -911,20 +919,14 @@ public void init_loop() {
public void start() {
follower.deactivateAllPIDFs();
follower.activateDrive();

forwards = follower.pathBuilder()
.setGlobalDeceleration()
.addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)))
.setConstantHeadingInterpolation(0)
.build();

backwards = follower.pathBuilder()
.setGlobalDeceleration()
.addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)))
.setConstantHeadingInterpolation(0)
.build();

follower.followPath(forwards);
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72+.001,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}

/**
Expand All @@ -936,19 +938,39 @@ public void loop() {
follower.update();
draw();

if (!follower.isBusy()) {
if (forward) {
forward = false;
follower.followPath(backwards);
} else {
forward = true;
follower.followPath(forwards);
}
if (gamepad1.dpadRightWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(0));
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72+impulseDistance,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}
if (gamepad1.dpadLeftWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(0));
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72-impulseDistance,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}
if (gamepad1.dpadUpWasPressed()) impulseDistance+=1;
if (gamepad1.dpadDownWasPressed() && impulseDistance-1 > 0) impulseDistance-=1;

telemetryM.debug("Driving forward?: " + forward);
telemetryM.addLine("Push dpad left/right to test the Drive PIDF(s) by driving "+impulseDistance+" inches.");
telemetryM.addLine("Push dpad up/down to adjust test distance by 1 inches.");
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]);
telemetryM.addData("Velocity Error (inches/sec)", follower.errorCalculator.getDriveErrors()[1]);
telemetryM.addData("Position Error (inches): ", follower.getCurrentPath().getDistanceRemaining());
telemetryM.addData("Drive power", follower.getDriveVector().getMagnitude());
telemetryM.update(telemetry);
}
}
Expand All @@ -964,11 +986,9 @@ public void loop() {
* @version 1.0, 3/12/2024
*/
class Line extends OpMode {
public static double DISTANCE = 40;
private boolean forward = true;
private int impulseDistance = 12;

private Path forwards;
private Path backwards;
private Path testPath;

@Override
public void init() {
Expand All @@ -979,8 +999,8 @@ public void init() {
@Override
public void init_loop() {
telemetryM.debug("This will activate all the PIDF(s)");
telemetryM.debug("The robot will go forward and backward continuously along the path while correcting.");
telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s).");
telemetryM.debug("Push dpad left/right to drive forward/backward by 12 inches.");
telemetryM.debug("Push dpad up/down to adjust the test distance by 1 inch.");
telemetryM.update(telemetry);
follower.update();
drawOnlyCurrent();
Expand All @@ -989,11 +1009,14 @@ public void init_loop() {
@Override
public void start() {
follower.activateAllPIDFs();
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72+.001,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}

/** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
Expand All @@ -1002,17 +1025,35 @@ public void loop() {
follower.update();
draw();

if (!follower.isBusy()) {
if (forward) {
forward = false;
follower.followPath(backwards);
} else {
forward = true;
follower.followPath(forwards);
}
if (gamepad1.dpadRightWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(0));
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72+impulseDistance,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}
if (gamepad1.dpadLeftWasPressed())
{
follower.setPose(new Pose(72, 72).withHeading(0));
testPath = new Path(new BezierLine(new Pose(72,72), new Pose(72-impulseDistance,72)));
testPath.setConstantHeadingInterpolation(0);
testPath.setTValueConstraint(1);
testPath.setVelocityConstraint(0);
testPath.setTranslationalConstraint(0);
testPath.setHeadingConstraint(0);
testPath.setTimeoutConstraint(100000);
follower.followPath(testPath);
}
if (gamepad1.dpadUpWasPressed()) impulseDistance+=1;
if (gamepad1.dpadDownWasPressed() && impulseDistance-1 > 0) impulseDistance-=1;

telemetryM.debug("Driving Forward?: " + forward);
telemetryM.addLine("Push dpad left/right to test all PIDFs by driving "+impulseDistance+" inches.");
telemetryM.debug("Push dpad up/down to adjust the test distance by 1 inch.");
telemetryM.update(telemetry);
}
}
Expand Down Expand Up @@ -1060,7 +1101,18 @@ public void init_loop() {
public void start() {
follower.activateAllPIDFs();
forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72)));
forwards.setTValueConstraint(1);
forwards.setVelocityConstraint(0);
forwards.setTranslationalConstraint(0);
forwards.setHeadingConstraint(0);
forwards.setTimeoutConstraint(2000);

backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72)));
backwards.setTValueConstraint(1);
backwards.setVelocityConstraint(0);
backwards.setTranslationalConstraint(0);
backwards.setHeadingConstraint(0);
backwards.setTimeoutConstraint(2000);

backwards.setTangentHeadingInterpolation();
backwards.reverseHeadingInterpolation();
Expand Down