KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

tuned heading pidf and fixed dash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-14 20:26:19 -05:00
parent 6c905f2506
commit f3efc132e7
2 changed files with 250 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,7 +19,8 @@ public class Constants {
.mass(15.5) .mass(15.5)
.forwardZeroPowerAcceleration(-29.512) .forwardZeroPowerAcceleration(-29.512)
.lateralZeroPowerAcceleration(-72.872) .lateralZeroPowerAcceleration(-72.872)
.translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)); .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012))
.headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02));
public static MecanumConstants driveConstants = new MecanumConstants() public static MecanumConstants driveConstants = new MecanumConstants()
.maxPower(1) .maxPower(1)
@@ -34,7 +35,8 @@ public class Constants {
.xVelocity(64.675) .xVelocity(64.675)
.yVelocity(49.583); .yVelocity(49.583);
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); public static double breakingStrength = 1.25;
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
public static PinpointConstants localizerConstants = new PinpointConstants() public static PinpointConstants localizerConstants = new PinpointConstants()
.forwardPodY(-7.5) .forwardPodY(-7.5)

514
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View File

@@ -8,12 +8,15 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import android.annotation.SuppressLint; import android.annotation.SuppressLint;
import com.acmerobotics.dashboard.config.Config;
import com.bylazar.configurables.PanelsConfigurables; import com.bylazar.configurables.PanelsConfigurables;
import com.bylazar.configurables.annotations.Configurable;
import com.bylazar.configurables.annotations.IgnoreConfigurable;
import com.bylazar.field.FieldManager; import com.bylazar.field.FieldManager;
import com.bylazar.field.PanelsField; import com.bylazar.field.PanelsField;
import com.bylazar.field.Style; import com.bylazar.field.Style;
@@ -44,19 +47,15 @@ import java.util.List;
* @author Baron Henderson - 20077 The Indubitables * @author Baron Henderson - 20077 The Indubitables
* @version 1.0, 6/26/2025 * @version 1.0, 6/26/2025
*/ */
@Configurable
@Config @Config
@TeleOp(name = "Tuning", group = "Pedro Pathing") @TeleOp(name = "Tuning", group = "Pedro Pathing")
public class Tuning extends SelectableOpMode { public class Tuning extends SelectableOpMode {
public static Follower follower; public static Follower follower;
@IgnoreConfigurable
static PoseHistory poseHistory; static PoseHistory poseHistory;
@IgnoreConfigurable
static TelemetryManager telemetryM; static TelemetryManager telemetryM;
@IgnoreConfigurable
static ArrayList<String> changes = new ArrayList<>(); static ArrayList<String> changes = new ArrayList<>();
public Tuning() { public Tuning() {
@@ -115,7 +114,7 @@ public class Tuning extends SelectableOpMode {
public static void drawCurrent() { public static void drawCurrent() {
try { try {
Drawing.drawRobot(follower.getPose()); Drawing.drawRobot(follower.getPose(), "blue");
Drawing.sendPacket(); Drawing.sendPacket();
} catch (Exception e) { } catch (Exception e) {
throw new RuntimeException("Drawing failed " + e); throw new RuntimeException("Drawing failed " + e);
@@ -123,7 +122,7 @@ public class Tuning extends SelectableOpMode {
} }
public static void drawCurrentAndHistory() { public static void drawCurrentAndHistory() {
Drawing.drawPoseHistory(poseHistory); Drawing.drawPoseHistory(poseHistory, "blue");
drawCurrent(); drawCurrent();
} }
@@ -160,11 +159,11 @@ class LocalizationTest extends OpMode {
} }
telemetryM.debug("This will print your robot's position to telemetry while " telemetry.addLine("This will print your robot's position to telemetry while "
+ "allowing robot control through a basic drive on gamepad 1."); + "allowing robot control through a basic drive on gamepad 1.");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)"); " (press gamepad a to toggle)");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -188,15 +187,15 @@ class LocalizationTest extends OpMode {
follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true);
follower.update(); follower.update();
telemetryM.debug("x:" + follower.getPose().getX()); telemetry.addLine("x:" + follower.getPose().getX());
telemetryM.debug("y:" + follower.getPose().getY()); telemetry.addLine("y:" + follower.getPose().getY());
telemetryM.debug("heading:" + follower.getPose().getHeading()); telemetry.addLine("heading:" + follower.getPose().getHeading());
telemetryM.debug("total heading:" + follower.getTotalHeading()); telemetry.addLine("total heading:" + follower.getTotalHeading());
if (debugStringEnabled) { if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" + telemetry.addLine("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString()); follower.getDrivetrain().debugString());
} }
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -228,8 +227,8 @@ class ForwardTuner extends OpMode {
/** This initializes the PoseUpdater as well as the Panels telemetry. */ /** This initializes the PoseUpdater as well as the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); telemetry.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry.");
telemetryM.update(telemetry); telemetry.update();
drawCurrent(); drawCurrent();
} }
@@ -241,10 +240,10 @@ class ForwardTuner extends OpMode {
public void loop() { public void loop() {
follower.update(); follower.update();
telemetryM.debug("Distance Moved: " + follower.getPose().getX()); telemetry.addLine("Distance Moved: " + follower.getPose().getX());
telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetry.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier())));
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -276,8 +275,8 @@ class LateralTuner extends OpMode {
/** This initializes the PoseUpdater as well as the Panels telemetry. */ /** This initializes the PoseUpdater as well as the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); telemetry.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry.");
telemetryM.update(telemetry); telemetry.update();
drawCurrent(); drawCurrent();
} }
@@ -289,10 +288,10 @@ class LateralTuner extends OpMode {
public void loop() { public void loop() {
follower.update(); follower.update();
telemetryM.debug("Distance Moved: " + follower.getPose().getY()); telemetry.addLine("Distance Moved: " + follower.getPose().getY());
telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetry.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier())));
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -324,8 +323,8 @@ class TurnTuner extends OpMode {
/** This initializes the PoseUpdater as well as the Panels telemetry. */ /** This initializes the PoseUpdater as well as the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); telemetry.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry.");
telemetryM.update(telemetry); telemetry.update();
drawCurrent(); drawCurrent();
} }
@@ -338,10 +337,10 @@ class TurnTuner extends OpMode {
public void loop() { public void loop() {
follower.update(); follower.update();
telemetryM.debug("Total Angle: " + follower.getTotalHeading()); telemetry.addLine("Total Angle: " + follower.getTotalHeading());
telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); telemetry.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians.");
telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); telemetry.addLine("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier())));
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -377,12 +376,12 @@ class ForwardVelocityTuner extends OpMode {
/** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
telemetryM.debug("Press B on game pad 1 to stop."); telemetry.addLine("Press B on game pad 1 to stop.");
telemetryM.debug("pose", follower.getPose()); telemetry.addData("pose", follower.getPose());
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
@@ -434,15 +433,15 @@ class ForwardVelocityTuner extends OpMode {
average += velocity; average += velocity;
} }
average /= velocities.size(); average /= velocities.size();
telemetryM.debug("Forward Velocity: " + average); telemetry.addLine("Forward Velocity: " + average);
telemetryM.debug("\n"); telemetry.addLine("\n");
telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); telemetry.addLine("Press A to set the Forward Velocity temporarily (while robot remains on).");
for (int i = 0; i < velocities.size(); i++) { for (int i = 0; i < velocities.size(); i++) {
telemetry.addData(String.valueOf(i), velocities.get(i)); telemetry.addData(String.valueOf(i), velocities.get(i));
} }
telemetryM.update(telemetry); telemetry.update();
telemetry.update(); telemetry.update();
if (gamepad1.aWasPressed()) { if (gamepad1.aWasPressed()) {
@@ -488,11 +487,11 @@ class LateralVelocityTuner extends OpMode {
*/ */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right.");
telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity.");
telemetryM.debug("Press B on Gamepad 1 to stop."); telemetry.addLine("Press B on Gamepad 1 to stop.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
@@ -542,10 +541,10 @@ class LateralVelocityTuner extends OpMode {
} }
average /= velocities.size(); average /= velocities.size();
telemetryM.debug("Strafe Velocity: " + average); telemetry.addLine("Strafe Velocity: " + average);
telemetryM.debug("\n"); telemetry.addLine("\n");
telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); telemetry.addLine("Press A to set the Lateral Velocity temporarily (while robot remains on).");
telemetryM.update(telemetry); telemetry.update();
if (gamepad1.aWasPressed()) { if (gamepad1.aWasPressed()) {
follower.setYVelocity(average); follower.setYVelocity(average);
@@ -589,12 +588,12 @@ class ForwardZeroPowerAccelerationTuner extends OpMode {
/** This initializes the drive motors as well as the Panels telemetryM. */ /** This initializes the drive motors as well as the Panels telemetryM. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); telemetry.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second.");
telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop.");
telemetryM.debug("Make sure you have enough room."); telemetry.addLine("Make sure you have enough room.");
telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); telemetry.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed.");
telemetryM.debug("Press B on Gamepad 1 to stop."); telemetry.addLine("Press B on Gamepad 1 to stop.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -648,10 +647,10 @@ class ForwardZeroPowerAccelerationTuner extends OpMode {
} }
average /= accelerations.size(); average /= accelerations.size();
telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); telemetry.addLine("Forward Zero Power Acceleration (Deceleration): " + average);
telemetryM.debug("\n"); telemetry.addLine("\n");
telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); telemetry.addLine("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on).");
telemetryM.update(telemetry); telemetry.update();
if (gamepad1.aWasPressed()) { if (gamepad1.aWasPressed()) {
follower.getConstants().setForwardZeroPowerAcceleration(average); follower.getConstants().setForwardZeroPowerAcceleration(average);
@@ -693,12 +692,12 @@ class LateralZeroPowerAccelerationTuner extends OpMode {
/** This initializes the drive motors as well as the Panels telemetry. */ /** This initializes the drive motors as well as the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); telemetry.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second.");
telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop.");
telemetryM.debug("Make sure you have enough room."); telemetry.addLine("Make sure you have enough room.");
telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); telemetry.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed.");
telemetryM.debug("Press B on game pad 1 to stop."); telemetry.addLine("Press B on game pad 1 to stop.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -752,10 +751,10 @@ class LateralZeroPowerAccelerationTuner extends OpMode {
} }
average /= accelerations.size(); average /= accelerations.size();
telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); telemetry.addLine("Lateral Zero Power Acceleration (Deceleration): " + average);
telemetryM.debug("\n"); telemetry.addLine("\n");
telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); telemetry.addLine("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on).");
telemetryM.update(telemetry); telemetry.update();
if (gamepad1.aWasPressed()) { if (gamepad1.aWasPressed()) {
follower.getConstants().setLateralZeroPowerAcceleration(average); follower.getConstants().setLateralZeroPowerAcceleration(average);
@@ -823,12 +822,12 @@ class PredictiveBrakingTuner extends OpMode {
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); telemetry.addLine("The robot will move forwards and backwards starting at max speed and slowing down.");
telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet."); telemetry.addLine("Make sure you have enough room. Leave at least 4-5 feet.");
telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); telemetry.addLine("After stopping, kFriction and kBraking will be displayed.");
telemetryM.debug("Make sure to turn the timer off."); telemetry.addLine("Make sure to turn the timer off.");
telemetryM.debug("Press B on game pad 1 to stop."); telemetry.addLine("Press B on game pad 1 to stop.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -906,10 +905,8 @@ class PredictiveBrakingTuner extends OpMode {
velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance});
telemetryM.debug("Test " + iteration, telemetry.addData("Test " + iteration, String.format("v=%.3f d=%.3f", measuredVelocity, brakingDistance));
String.format("v=%.3f d=%.3f", measuredVelocity, telemetry.update();
brakingDistance));
telemetryM.update(telemetry);
iteration++; iteration++;
state = State.START_MOVE; state = State.START_MOVE;
@@ -922,23 +919,23 @@ class PredictiveBrakingTuner extends OpMode {
double[] coefficients = quadraticFit(velocityToBrakingDistance); double[] coefficients = quadraticFit(velocityToBrakingDistance);
telemetryM.debug("Tuning Complete"); telemetry.addLine("Tuning Complete");
telemetryM.debug("Braking Profile:"); telemetry.addLine("Braking Profile:");
telemetryM.debug("kQuadratic", coefficients[1]); telemetry.addData("kQuadratic", coefficients[1]);
telemetryM.debug("kLinear", coefficients[0]); telemetry.addData("kLinear", coefficients[0]);
telemetryM.update(telemetry); telemetry.update();
telemetryM.debug("Tuning Complete"); telemetry.addLine("Tuning Complete");
telemetryM.debug("Braking Profile:"); telemetry.addLine("Braking Profile:");
telemetryM.debug("kQuadraticFriction", coefficients[1]); telemetry.addData("kQuadraticFriction", coefficients[1]);
telemetryM.debug("kLinearBraking", coefficients[0]); telemetry.addData("kLinearBraking", coefficients[0]);
for (BrakeRecord record : brakeData) { for (BrakeRecord record : brakeData) {
Pose p = record.pose; Pose p = record.pose;
telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", telemetry.addLine(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f",
record.timeMs, p.getX(), p.getY(), record.timeMs, p.getX(), p.getY(),
p.getHeading(), p.getHeading(),
record.velocity)); record.velocity));
} }
telemetryM.update(); telemetry.update();
break; break;
} }
} }
@@ -970,10 +967,10 @@ class TranslationalTuner extends OpMode {
/** This initializes the Follower and creates the forward and backward Paths. */ /** This initializes the Follower and creates the forward and backward Paths. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will activate the translational PIDF(s)"); telemetry.addLine("This will activate the translational PIDF(s)");
telemetryM.debug("The robot will try to stay in place while you push it laterally."); telemetry.addLine("The robot will try to stay in place while you push it laterally.");
telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); telemetry.addLine("You can adjust the PIDF values to tune the robot's translational PIDF(s).");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1005,11 +1002,11 @@ class TranslationalTuner extends OpMode {
} }
} }
telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); telemetry.addLine("Push the robot laterally to test the Translational PIDF(s).");
telemetryM.addData("Zero Line", 0); telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent());
telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent());
telemetryM.update(telemetry); telemetry.update();
} }
} }
@@ -1042,10 +1039,10 @@ class HeadingTuner extends OpMode {
*/ */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will activate the heading PIDF(s)."); telemetry.addLine("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."); telemetry.addLine("The robot will try to stay at a constant heading while you try to turn it.");
telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); telemetry.addLine("You can adjust the PIDF values to tune the robot's heading PIDF(s).");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1080,10 +1077,10 @@ class HeadingTuner extends OpMode {
} }
} }
telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); telemetry.addLine("Turn the robot manually to test the Heading PIDF(s).");
telemetryM.addData("Zero Line", 0); telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); telemetryM.addData("Error", follower.errorCalculator.getHeadingError());
telemetryM.update(telemetry); telemetry.update();
} }
} }
@@ -1114,10 +1111,10 @@ class DriveTuner extends OpMode {
*/ */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); telemetry.addLine("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."); telemetry.addLine("The robot will go forward and backward continuously along the path.");
telemetryM.debug("Make sure you have enough room."); telemetry.addLine("Make sure you have enough room.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1161,10 +1158,10 @@ class DriveTuner extends OpMode {
} }
} }
telemetryM.debug("Driving forward?: " + forward); telemetry.addLine("Driving forward?: " + forward);
telemetryM.addData("Zero Line", 0); telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]);
telemetryM.update(telemetry); telemetry.update();
} }
} }
@@ -1193,10 +1190,10 @@ class Line extends OpMode {
/** This initializes the Follower and creates the forward and backward Paths. */ /** This initializes the Follower and creates the forward and backward Paths. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will activate all the PIDF(s)"); telemetry.addLine("This will activate all the PIDF(s)");
telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); telemetry.addLine("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)."); telemetry.addLine("You can adjust the PIDF values to tune the robot's drive PIDF(s).");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1227,8 +1224,8 @@ class Line extends OpMode {
} }
} }
telemetryM.debug("Driving Forward?: " + forward); telemetry.addLine("Driving Forward?: " + forward);
telemetryM.update(telemetry); telemetry.update();
} }
} }
@@ -1263,10 +1260,10 @@ class CentripetalTuner extends OpMode {
*/ */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); telemetry.addLine("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward.");
telemetryM.debug("The robot will go continuously along the path."); telemetry.addLine("The robot will go continuously along the path.");
telemetryM.debug("Make sure you have enough room."); telemetry.addLine("Make sure you have enough room.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1301,8 +1298,8 @@ class CentripetalTuner extends OpMode {
} }
} }
telemetryM.debug("Driving away from the origin along the curve?: " + forward); telemetry.addLine("Driving away from the origin along the curve?: " + forward);
telemetryM.update(telemetry); telemetry.update();
} }
} }
@@ -1343,9 +1340,9 @@ class Triangle extends OpMode {
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); telemetry.addLine("This will run in a roughly triangular shape, starting on the bottom-middle point.");
telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); telemetry.addLine("So, make sure you have enough space to the left, front, and right to run the OpMode.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1399,10 +1396,10 @@ class Circle extends OpMode {
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); telemetry.addLine("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. ");
telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); telemetry.addLine("So, make sure you have enough space to the left, front, and back to run the OpMode.");
telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); telemetry.addLine("It will also continuously face the center of the circle to test your heading and centripetal correction.");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1445,9 +1442,9 @@ class AnalogMinMaxTuner extends OpMode {
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + telemetry.addLine("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" +
"The OpMode will keep track of the min and max voltages seen so far and print them to telemetry."); "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.");
telemetryM.update(telemetry); telemetry.update();
} }
@Override @Override
@@ -1473,7 +1470,7 @@ class AnalogMinMaxTuner extends OpMode {
hub.clearBulkCache(); hub.clearBulkCache();
} }
telemetryM.debug("Spin each pod slowly for 4 to 5 full rotations.\n" + telemetry.addLine("Spin each pod slowly for 4 to 5 full rotations.\n" +
"The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n"); "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n");
for (int i = 0; i < encoders.length; i++) { for (int i = 0; i < encoders.length; i++) {
@@ -1485,7 +1482,7 @@ class AnalogMinMaxTuner extends OpMode {
telemetryM.addLine(""); telemetryM.addLine("");
} }
telemetryM.update(); telemetry.update();
} }
} }
@@ -1509,11 +1506,11 @@ class SwerveOffsetsTest extends OpMode {
} }
telemetryM.debug("This OpMode will run all four swerve pods in the direction they think is forward" telemetry.addLine("This OpMode will run all four swerve pods in the direction they think is forward"
+ "\nensure your bot is not on the ground while running"); + "\nensure your bot is not on the ground while running");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)"); " (press gamepad a to toggle)");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1538,10 +1535,10 @@ class SwerveOffsetsTest extends OpMode {
follower.update(); follower.update();
if (debugStringEnabled) { if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" + telemetry.addLine("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString()); follower.getDrivetrain().debugString());
} }
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -1567,11 +1564,11 @@ class SwerveTurnTest extends OpMode {
} }
telemetryM.debug("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " telemetry.addLine("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) "
+ "\nrun this once off the ground to check servo directions and motor directions before testing on the ground"); + "\nrun this once off the ground to check servo directions and motor directions before testing on the ground");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)"); " (press gamepad a to toggle)");
telemetryM.update(telemetry); telemetry.update();
follower.update(); follower.update();
drawCurrent(); drawCurrent();
} }
@@ -1596,10 +1593,10 @@ class SwerveTurnTest extends OpMode {
follower.update(); follower.update();
if (debugStringEnabled) { if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" + telemetry.addLine("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString()); follower.getDrivetrain().debugString());
} }
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -1624,9 +1621,9 @@ class OffsetsTuner extends OpMode {
/** This initializes the PoseUpdater as well as the Panels telemetry. */ /** This initializes the PoseUpdater as well as the Panels telemetry. */
@Override @Override
public void init_loop() { public void init_loop() {
telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); telemetry.addLine("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants.");
telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); telemetry.addLine("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry.");
telemetryM.update(telemetry); telemetry.update();
drawCurrent(); drawCurrent();
} }
@@ -1639,12 +1636,12 @@ class OffsetsTuner extends OpMode {
public void loop() { public void loop() {
follower.update(); follower.update();
telemetryM.debug("Total Angle: " + follower.getTotalHeading()); telemetry.addLine("Total Angle: " + follower.getTotalHeading());
telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer."); telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer.");
telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0));
telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0));
telemetryM.update(telemetry); telemetry.update();
drawCurrentAndHistory(); drawCurrentAndHistory();
} }
@@ -1652,147 +1649,126 @@ class OffsetsTuner extends OpMode {
/** /**
* This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot.
* *
* @author Lazar - 19234 * @author Logan Nash
* @version 1.1, 5/19/2025 * @author Anyi Lin - 10158 Scott's Bots
* @version 2.0, 11/03/2025
*/ */
class Drawing { class Drawing {
public static final double ROBOT_RADIUS = 9; // woah public static final double ROBOT_RADIUS = 9;
private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); private static TelemetryPacket packet;
private static final Style robotLook = new Style(
"", "#3F51B5", 0.75
);
private static final Style historyLook = new Style(
"", "#4CAF50", 0.75
);
/**
* This prepares Panels Field for using Pedro Offsets
*/
public static void init() {
panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
}
/** /**
* This draws everything that will be used in the Follower's telemetryDebug() method. This takes * 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. * a Follower as an input, so an instance of the DashboardDrawingHandler class is not needed.
* *
* @param follower Pedro Follower instance. * @param follower
*/ */
public static void drawDebug(Follower follower) { public static void drawDebug(Follower follower) {
if (follower.getCurrentPath() != null) { if (follower.getCurrentPath() != null) {
drawPath(follower.getCurrentPath(), robotLook); drawPath(follower.getCurrentPath(), "#3F51B5");
Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue());
drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5");
} }
drawPoseHistory(follower.getPoseHistory(), historyLook); drawPoseHistory(follower.getPoseHistory(), "#4CAF50");
drawRobot(follower.getPose(), historyLook); drawRobot(follower.getPose(), "#4CAF50");
sendPacket(); sendPacket();
} }
/** /**
* This draws a robot at a specified Pose with a specified * This adds instructions to the current packet to draw a robot at a specified Pose with a specified
* look. The heading is represented as a line. * color. If no packet exists, then a new one is created.
* *
* @param pose the Pose to draw the robot at * @param pose the Pose to draw the robot at
* @param style the parameters used to draw the robot with * @param color the color to draw the robot with
*/ */
public static void drawRobot(Pose pose, Style style) { public static void drawRobot(Pose pose, String color) {
if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { if (packet == null) packet = new TelemetryPacket();
packet.fieldOverlay().setStroke(color);
Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy());
}
/**
* This adds instructions to the current packet to draw a Path with a specified color. If no
* packet exists, then a new one is created.
*
* @param path the Path to draw
* @param color the color to draw the Path with
*/
public static void drawPath(Path path, String color) {
if (packet == null) packet = new TelemetryPacket();
packet.fieldOverlay().setStroke(color);
Drawing.drawPath(packet.fieldOverlay(), path.getPanelsDrawingPoints());
}
/**
* This adds instructions to the current packet to draw all the Paths in a PathChain with a
* specified color. If no packet exists, then a new one is created.
*
* @param pathChain the PathChain to draw
* @param color the color to draw the PathChain with
*/
public static void drawPath(PathChain pathChain, String color) {
for (int i = 0; i < pathChain.size(); i++) {
drawPath(pathChain.getPath(i), color);
}
}
/**
* This adds instructions to the current packet to draw the pose history of the robot. If no
* packet exists, then a new one is created.
*
* @param poseTracker the DashboardPoseTracker to get the pose history from
* @param color the color to draw the pose history with
*/
public static void drawPoseHistory(PoseHistory poseTracker, String color) {
if (packet == null) packet = new TelemetryPacket();
packet.fieldOverlay().setStroke(color);
packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray());
}
/**
* This tries to send the current packet to FTC Dashboard.
*
* @return returns if the operation was successful.
*/
public static boolean sendPacket() {
if (packet != null) {
FtcDashboard.getInstance().sendTelemetryPacket(packet);
packet = null;
return true;
}
return false;
}
/**
* This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the
* actual robot, since the Pose contains the direction the robot is facing as well as its position.
*
* @param c the Canvas on the Dashboard on which this will draw at
* @param t the Pose to draw at
*/
public static void drawRobotOnCanvas(Canvas c, Pose t) {
if (t == null || Double.isNaN(t.getX()) || Double.isNaN(t.getY()) || Double.isNaN(t.getHeading())) {
return; return;
} }
panelsField.setStyle(style); c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
panelsField.moveCursor(pose.getX(), pose.getY()); Vector v = t.getHeadingAsUnitVector();
panelsField.circle(ROBOT_RADIUS);
Vector v = pose.getHeadingAsUnitVector();
v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2;
double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent();
c.strokeLine(x1, y1, x2, y2);
panelsField.setStyle(style);
panelsField.moveCursor(x1, y1);
panelsField.line(x2, y2);
} }
/** /**
* This draws a robot at a specified Pose. The heading is represented as a line. * This draws a Path on the Dashboard from a specified Array of Points.
* *
* @param pose the Pose to draw the robot at * @param c the Canvas on the Dashboard on which this will draw
* @param points the Points to draw
*/ */
public static void drawRobot(Pose pose) { public static void drawPath(Canvas c, double[][] points) {
drawRobot(pose, robotLook); c.strokePolyline(points[0], points[1]);
}
/**
* This draws a Path with a specified look.
*
* @param path the Path to draw
* @param style the parameters used to draw the Path with
*/
public static void drawPath(Path path, Style style) {
double[][] points = path.getPanelsDrawingPoints();
for (int i = 0; i < points[0].length; i++) {
for (int j = 0; j < points.length; j++) {
if (Double.isNaN(points[j][i])) {
points[j][i] = 0;
}
}
}
panelsField.setStyle(style);
panelsField.moveCursor(points[0][0], points[0][1]);
panelsField.line(points[1][0], points[1][1]);
}
/**
* This draws all the Paths in a PathChain with a
* specified look.
*
* @param pathChain the PathChain to draw
* @param style the parameters used to draw the PathChain with
*/
public static void drawPath(PathChain pathChain, Style style) {
for (int i = 0; i < pathChain.size(); i++) {
drawPath(pathChain.getPath(i), style);
}
}
/**
* This draws the pose history of the robot.
*
* @param poseTracker the PoseHistory to get the pose history from
* @param style the parameters used to draw the pose history with
*/
public static void drawPoseHistory(PoseHistory poseTracker, Style style) {
panelsField.setStyle(style);
int size = poseTracker.getXPositionsArray().length;
for (int i = 0; i < size - 1; i++) {
panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]);
panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]);
}
}
/**
* This draws the pose history of the robot.
*
* @param poseTracker the PoseHistory to get the pose history from
*/
public static void drawPoseHistory(PoseHistory poseTracker) {
drawPoseHistory(poseTracker, historyLook);
}
/**
* This tries to send the current packet to FTControl Panels.
*/
public static void sendPacket() {
panelsField.update();
} }
} }