diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 5a30fe1..2190bff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -19,7 +19,8 @@ public class Constants { .mass(15.5) .forwardZeroPowerAcceleration(-29.512) .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() .maxPower(1) @@ -34,7 +35,8 @@ public class Constants { .xVelocity(64.675) .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() .forwardPodY(-7.5) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 1258ea1..0de897d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -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.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 com.acmerobotics.dashboard.config.Config; 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.PanelsField; import com.bylazar.field.Style; @@ -44,19 +47,15 @@ import java.util.List; * @author Baron Henderson - 20077 The Indubitables * @version 1.0, 6/26/2025 */ -@Configurable @Config @TeleOp(name = "Tuning", group = "Pedro Pathing") public class Tuning extends SelectableOpMode { public static Follower follower; - @IgnoreConfigurable static PoseHistory poseHistory; - @IgnoreConfigurable static TelemetryManager telemetryM; - @IgnoreConfigurable static ArrayList changes = new ArrayList<>(); public Tuning() { @@ -115,7 +114,7 @@ public class Tuning extends SelectableOpMode { public static void drawCurrent() { try { - Drawing.drawRobot(follower.getPose()); + Drawing.drawRobot(follower.getPose(), "blue"); Drawing.sendPacket(); } catch (Exception e) { throw new RuntimeException("Drawing failed " + e); @@ -123,7 +122,7 @@ public class Tuning extends SelectableOpMode { } public static void drawCurrentAndHistory() { - Drawing.drawPoseHistory(poseHistory); + Drawing.drawPoseHistory(poseHistory, "blue"); 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."); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); 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.update(); - telemetryM.debug("x:" + follower.getPose().getX()); - telemetryM.debug("y:" + follower.getPose().getY()); - telemetryM.debug("heading:" + follower.getPose().getHeading()); - telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetry.addLine("x:" + follower.getPose().getX()); + telemetry.addLine("y:" + follower.getPose().getY()); + telemetry.addLine("heading:" + follower.getPose().getHeading()); + telemetry.addLine("total heading:" + follower.getTotalHeading()); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); drawCurrentAndHistory(); } @@ -228,8 +227,8 @@ class ForwardTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -241,10 +240,10 @@ class ForwardTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("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."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getX()); + telemetry.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -276,8 +275,8 @@ class LateralTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override 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."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -289,10 +288,10 @@ class LateralTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("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."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getY()); + telemetry.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -324,8 +323,8 @@ class TurnTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -338,10 +337,10 @@ class TurnTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("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."); - telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Total Angle: " + follower.getTotalHeading()); + telemetry.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetry.addLine("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetry.update(); 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. */ @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("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."); - telemetryM.debug("pose", follower.getPose()); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.addData("pose", follower.getPose()); + telemetry.update(); follower.update(); drawCurrent(); @@ -434,15 +433,15 @@ class ForwardVelocityTuner extends OpMode { average += velocity; } average /= velocities.size(); - telemetryM.debug("Forward Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + telemetry.addLine("Forward Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Velocity temporarily (while robot remains on)."); for (int i = 0; i < velocities.size(); i++) { telemetry.addData(String.valueOf(i), velocities.get(i)); } - telemetryM.update(telemetry); + telemetry.update(); telemetry.update(); if (gamepad1.aWasPressed()) { @@ -488,11 +487,11 @@ class LateralVelocityTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("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."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); @@ -542,10 +541,10 @@ class LateralVelocityTuner extends OpMode { } average /= velocities.size(); - telemetryM.debug("Strafe Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Strafe Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.setYVelocity(average); @@ -589,12 +588,12 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { /** This initializes the drive motors as well as the Panels telemetryM. */ @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -648,10 +647,10 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Forward Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setForwardZeroPowerAcceleration(average); @@ -693,12 +692,12 @@ class LateralZeroPowerAccelerationTuner extends OpMode { /** This initializes the drive motors as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -752,10 +751,10 @@ class LateralZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setLateralZeroPowerAcceleration(average); @@ -823,12 +822,12 @@ class PredictiveBrakingTuner extends OpMode { @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); - telemetryM.debug("Make sure to turn the timer off."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will move forwards and backwards starting at max speed and slowing down."); + telemetry.addLine("Make sure you have enough room. Leave at least 4-5 feet."); + telemetry.addLine("After stopping, kFriction and kBraking will be displayed."); + telemetry.addLine("Make sure to turn the timer off."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -906,10 +905,8 @@ class PredictiveBrakingTuner extends OpMode { velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); - telemetryM.debug("Test " + iteration, - String.format("v=%.3f d=%.3f", measuredVelocity, - brakingDistance)); - telemetryM.update(telemetry); + telemetry.addData("Test " + iteration, String.format("v=%.3f d=%.3f", measuredVelocity, brakingDistance)); + telemetry.update(); iteration++; state = State.START_MOVE; @@ -922,23 +919,23 @@ class PredictiveBrakingTuner extends OpMode { double[] coefficients = quadraticFit(velocityToBrakingDistance); - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadratic", coefficients[1]); - telemetryM.debug("kLinear", coefficients[0]); - telemetryM.update(telemetry); - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadraticFriction", coefficients[1]); - telemetryM.debug("kLinearBraking", coefficients[0]); + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadratic", coefficients[1]); + telemetry.addData("kLinear", coefficients[0]); + telemetry.update(); + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadraticFriction", coefficients[1]); + telemetry.addData("kLinearBraking", coefficients[0]); for (BrakeRecord record : brakeData) { 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(), p.getHeading(), record.velocity)); } - telemetryM.update(); + telemetry.update(); break; } } @@ -970,10 +967,10 @@ class TranslationalTuner extends OpMode { /** 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("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the translational PIDF(s)"); + telemetry.addLine("The robot will try to stay in place while you push it laterally."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetry.update(); follower.update(); 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("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); - telemetryM.update(telemetry); + telemetry.update(); } } @@ -1042,10 +1039,10 @@ class HeadingTuner extends OpMode { */ @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("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the heading PIDF(s)."); + telemetry.addLine("The robot will try to stay at a constant heading while you try to turn it."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetry.update(); follower.update(); 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("Error", follower.errorCalculator.getHeadingError()); - telemetryM.update(telemetry); + telemetry.update(); } } @@ -1114,10 +1111,10 @@ class DriveTuner extends OpMode { */ @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.update(telemetry); + telemetry.addLine("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetry.addLine("The robot will go forward and backward continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); 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("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. */ @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.update(telemetry); + telemetry.addLine("This will activate all the PIDF(s)"); + telemetry.addLine("The robot will go forward and backward continuously along the path while correcting."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1227,8 +1224,8 @@ class Line extends OpMode { } } - telemetryM.debug("Driving Forward?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving Forward?: " + forward); + telemetry.update(); } } @@ -1263,10 +1260,10 @@ class CentripetalTuner extends OpMode { */ @Override 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."); - telemetryM.debug("The robot will go continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); + telemetry.addLine("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetry.addLine("The robot will go continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1301,8 +1298,8 @@ class CentripetalTuner extends OpMode { } } - telemetryM.debug("Driving away from the origin along the curve?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving away from the origin along the curve?: " + forward); + telemetry.update(); } } @@ -1343,9 +1340,9 @@ class Triangle extends OpMode { @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetry.addLine("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1399,10 +1396,10 @@ class Circle extends OpMode { @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetry.addLine("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetry.addLine("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1445,9 +1442,9 @@ class AnalogMinMaxTuner extends OpMode { @Override 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."); - telemetryM.update(telemetry); + telemetry.update(); } @Override @@ -1473,7 +1470,7 @@ class AnalogMinMaxTuner extends OpMode { 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"); for (int i = 0; i < encoders.length; i++) { @@ -1485,7 +1482,7 @@ class AnalogMinMaxTuner extends OpMode { 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"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1538,10 +1535,10 @@ class SwerveOffsetsTest extends OpMode { follower.update(); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); 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"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1596,10 +1593,10 @@ class SwerveTurnTest extends OpMode { follower.update(); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); drawCurrentAndHistory(); } @@ -1624,9 +1621,9 @@ class OffsetsTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("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."); - telemetryM.update(telemetry); + telemetry.addLine("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); + telemetry.addLine("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -1639,12 +1636,12 @@ class OffsetsTuner extends OpMode { public void loop() { 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."); - telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); - telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); - telemetryM.update(telemetry); + telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer."); + telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); + telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetry.update(); 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 - * @version 1.1, 5/19/2025 + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 2.0, 11/03/2025 */ class Drawing { - public static final double ROBOT_RADIUS = 9; // woah - private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); - - 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()); - } + 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. + * 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) { if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), robotLook); + drawPath(follower.getCurrentPath(), "#3F51B5"); 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); - drawRobot(follower.getPose(), historyLook); - + drawPoseHistory(follower.getPoseHistory(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); sendPacket(); } /** - * This draws a robot at a specified Pose with a specified - * look. The heading is represented as a line. + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. * - * @param pose the Pose to draw the robot at - * @param style the parameters used to draw the robot with + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with */ - public static void drawRobot(Pose pose, Style style) { - if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + public static void drawRobot(Pose pose, String color) { + 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; } - panelsField.setStyle(style); - panelsField.moveCursor(pose.getX(), pose.getY()); - panelsField.circle(ROBOT_RADIUS); - - Vector v = pose.getHeadingAsUnitVector(); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingAsUnitVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; - double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); - - panelsField.setStyle(style); - panelsField.moveCursor(x1, y1); - panelsField.line(x2, y2); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, 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) { - drawRobot(pose, robotLook); - } - - /** - * 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(); + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); } } \ No newline at end of file