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 c35327ad10b6..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); }); }); } @@ -725,7 +726,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 +737,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, @@ -749,37 +750,34 @@ 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; this.velocity = velocity; } } - - 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; - + 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."); @@ -791,47 +789,43 @@ 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) { 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); - } - + 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(); @@ -840,57 +834,55 @@ public void loop() { } break; } - + case APPLY_BRAKE: { - double reversePower = -lastDriveSign * follower.constants.predictiveBrakingCoefficients.maximumBrakingPower; - follower.setTeleOpDrive(reversePower, 0, 0, true); + 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(); 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) { + + 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)); + 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:"); @@ -899,9 +891,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; @@ -1182,6 +1174,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