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..c35327ad10b6 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 @@ -749,12 +749,12 @@ private enum State { RECORD, DONE } - + private static class BrakeRecord { double timeMs; Pose pose; double velocity; - + BrakeRecord(double timeMs, Pose pose, double velocity) { this.timeMs = timeMs; this.pose = pose; @@ -762,12 +762,15 @@ private static class BrakeRecord { } } + private double lastDriveSign = 1.0; + private static final int BRAKE_PULSE_MS = 150; + private State state = State.START_MOVE; private final ElapsedTime timer = new ElapsedTime(); private int iteration = 0; - + private Vector startPosition; private double measuredVelocity; @@ -813,12 +816,14 @@ public void loop() { state = State.DONE; break; } - + double currentPower = TEST_POWERS[iteration]; follower.setMaxPower(currentPower); if (iteration % 2 != 0) { + lastDriveSign = -1.0; follower.setTeleOpDrive(-1, 0, 0, true); } else { + lastDriveSign = 1.0; follower.setTeleOpDrive(1, 0, 0, true); } @@ -837,7 +842,8 @@ public void loop() { } case APPLY_BRAKE: { - stopRobot(); + double reversePower = -lastDriveSign * follower.constants.predictiveBrakingCoefficients.maximumBrakingPower; + follower.setTeleOpDrive(reversePower, 0, 0, true); timer.reset(); state = State.WAIT_BRAKE_TIME; @@ -848,9 +854,13 @@ public void loop() { double t = timer.milliseconds(); Pose currentPose = follower.getPose(); double currentVelocity = follower.getVelocity().getMagnitude(); - + brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); - + + if (t >= BRAKE_PULSE_MS) { + stopRobot(); + } + if (timer.milliseconds() >= BRAKE_WAIT_MS || follower.getVelocity().getMagnitude() <= .05) { state = State.RECORD; } @@ -864,8 +874,8 @@ public void loop() { velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); telemetryM.debug("Test " + iteration, - String.format("v=%.3f d=%.3f", measuredVelocity, - brakingDistance)); + String.format("v=%.3f d=%.3f", measuredVelocity, + brakingDistance)); telemetryM.update(telemetry); iteration++; @@ -889,9 +899,9 @@ public void loop() { for (BrakeRecord record : brakeData) { Pose p = record.pose; telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", - record.timeMs, p.getX(), p.getY(), - p.getHeading(), - record.velocity)); + record.timeMs, p.getX(), p.getY(), + p.getHeading(), + record.velocity)); } telemetryM.update(); break;