Skip to content

Reverse Power for PredictiveBrakingTuner #50

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
junkjunk123 merged 3 commits into from
Jan 4, 2026
Merged

Reverse Power for PredictiveBrakingTuner #50

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fbc0b2b
PredictiveBrakingTuner applies reverse voltage + Line90DegreeTurn
jjophoven Jan 2, 2026
512f7f3
add Line90DegreeTurn
jjophoven Jan 2, 2026
b7bc523
Merge remote-tracking branch 'upstream/dev' into dev
jjophoven Jan 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
146 changes: 90 additions & 56 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
});
});
}
Expand Down Expand Up @@ -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.
Expand All @@ -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,
Expand All @@ -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<double[]> velocityToBrakingDistance = new ArrayList<>();
private final List<BrakeRecord> 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.");
Expand All @@ -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();
Expand All @@ -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:");
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down