From fbc0b2bccb615a66e64845d9267f8cf9bcb8379f Mon Sep 17 00:00:00 2001 From: PythonDominator Date: Fri, 2 Jan 2026 14:20:59 -0600 Subject: [PATCH 1/2] PredictiveBrakingTuner applies reverse voltage + Line90DegreeTurn --- .../ftc/teamcode/pedroPathing/Tuning.java | 113 ++++++++++++------ 1 file changed, 78 insertions(+), 35 deletions(-) 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 50640b2caaea..8f20a07411d5 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 @@ -725,7 +725,7 @@ public void loop() { /** * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power * levels, recording the robot’s velocity and position immediately before braking. The motors are - * then set to zero-power brake mode, which represents the fastest theoretical braking the robot + * then set to a reverse power, which represents the fastest theoretical braking the robot * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance. * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a * quadratic curve to model the braking behavior. @@ -736,10 +736,10 @@ public void loop() { */ class PredictiveBrakingTuner extends OpMode { private static final double[] TEST_POWERS = - {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; - - private static final int DRIVE_TIME_MS = 2000; - private static final int BRAKE_WAIT_MS = 1000; + {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; + private static final double BRAKING_POWER = -0.2; + + private static final int DRIVE_TIME_MS = 1000; private enum State { START_MOVE, @@ -761,22 +761,22 @@ private static class BrakeRecord { this.velocity = velocity; } } - + private State state = State.START_MOVE; - + private final ElapsedTime timer = new ElapsedTime(); - + private int iteration = 0; private Vector startPosition; private double measuredVelocity; - + private final List velocityToBrakingDistance = new ArrayList<>(); private final List brakeData = new ArrayList<>(); - + @Override public void init() {} - + @Override public void init_loop() { telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); @@ -788,25 +788,27 @@ public void init_loop() { follower.update(); drawCurrent(); } - + @Override public void start() { timer.reset(); follower.update(); follower.startTeleOpDrive(true); } - + @SuppressLint("DefaultLocale") @Override public void loop() { follower.update(); - + if (gamepad1.b) { stopRobot(); requestOpModeStop(); return; } - + + double direction = (iteration % 2 == 0) ? 1 : -1; + switch (state) { case START_MOVE: { if (iteration >= TEST_POWERS.length) { @@ -816,17 +818,13 @@ public void loop() { double currentPower = TEST_POWERS[iteration]; follower.setMaxPower(currentPower); - if (iteration % 2 != 0) { - follower.setTeleOpDrive(-1, 0, 0, true); - } else { - follower.setTeleOpDrive(1, 0, 0, true); - } - + follower.setTeleOpDrive(direction, 0, 0, true); + timer.reset(); state = State.WAIT_DRIVE_TIME; break; } - + case WAIT_DRIVE_TIME: { if (timer.milliseconds() >= DRIVE_TIME_MS) { measuredVelocity = follower.getVelocity().getMagnitude(); @@ -835,15 +833,15 @@ public void loop() { } break; } - + case APPLY_BRAKE: { - stopRobot(); + follower.setTeleOpDrive(BRAKING_POWER * direction, 0, 0, true); timer.reset(); state = State.WAIT_BRAKE_TIME; break; } - + case WAIT_BRAKE_TIME: { double t = timer.milliseconds(); Pose currentPose = follower.getPose(); @@ -851,36 +849,39 @@ public void loop() { brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); - if (timer.milliseconds() >= BRAKE_WAIT_MS || follower.getVelocity().getMagnitude() <= .05) { + if (follower.getVelocity().dot(new Vector(direction, + follower.getHeading())) <= 0) { state = State.RECORD; } break; } - + case RECORD: { Vector endPosition = follower.getPose().getAsVector(); double brakingDistance = endPosition.minus(startPosition).getMagnitude(); - + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); - + telemetryM.debug("Test " + iteration, String.format("v=%.3f d=%.3f", measuredVelocity, brakingDistance)); telemetryM.update(telemetry); - + iteration++; state = State.START_MOVE; - + break; } - + case DONE: { + stopRobot(); + double[] coefficients = quadraticFit(velocityToBrakingDistance); - + telemetryM.debug("Tuning Complete"); telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadraticFriction", coefficients[1]); - telemetryM.debug("kLinearBraking", coefficients[0]); + telemetryM.debug("kQuadratic", coefficients[1]); + telemetryM.debug("kLinear", coefficients[0]); telemetryM.update(telemetry); telemetryM.debug("Tuning Complete"); telemetryM.debug("Braking Profile:"); @@ -1172,6 +1173,48 @@ public void loop() { } } +/** + * @author Jacob Ophoven - 18535 Frozen Code + */ +class Line90DegreeTurn extends OpMode { + @Override + public void init() {} + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will go forward 48 inches and then sideways to " + + "the left 24 inches."); + telemetryM.update(telemetry); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + Path forwards = new Path(new BezierLine(new Pose(0, 0), new Pose(48, 0))); + forwards.setConstantHeadingInterpolation(0); + Path sideways = new Path(new BezierLine(new Pose(48, 0), new Pose(48, + 24))); + sideways.setConstantHeadingInterpolation(0); + follower.followPath(new PathChain(forwards, sideways)); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + drawCurrentAndHistory(); + + if (!follower.isBusy()) { + stopRobot(); + } + + telemetryM.update(telemetry); + } +} + /** * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance * forward and to the left. On reaching the end of the forward Path, the robot runs the backward From 512f7f34e6c2ff4eab985b32c039a83866efba2d Mon Sep 17 00:00:00 2001 From: PythonDominator Date: Fri, 2 Jan 2026 14:45:01 -0600 Subject: [PATCH 2/2] add Line90DegreeTurn --- .../java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java | 1 + 1 file changed, 1 insertion(+) 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 8f20a07411d5..cdb154f38a2e 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 @@ -77,6 +77,7 @@ public Tuning() { p.add("Line", Line::new); p.add("Triangle", Triangle::new); p.add("Circle", Circle::new); + p.add("Line90DegreeTurn", Line90DegreeTurn::new); }); }); }