Skip to content

Commit 20d5c2e

Browse files
BaronClapsBaronClaps
committed
Pedro 2.1.0
1 parent dc603d8 commit 20d5c2e

2 files changed

Lines changed: 235 additions & 9 deletions

File tree

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java‎

Lines changed: 233 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,9 @@
11
package org.firstinspires.ftc.teamcode.pedroPathing;
22

3-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes;
4-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
5-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw;
6-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
7-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
8-
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
3+
import static com.pedropathing.math.MathFunctions.quadraticFit;
4+
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.*;
95

6+
import android.annotation.SuppressLint;
107
import com.bylazar.configurables.PanelsConfigurables;
118
import com.bylazar.configurables.annotations.Configurable;
129
import com.bylazar.configurables.annotations.IgnoreConfigurable;
@@ -15,7 +12,6 @@
1512
import com.bylazar.field.Style;
1613
import com.bylazar.telemetry.PanelsTelemetry;
1714
import com.bylazar.telemetry.TelemetryManager;
18-
import com.pedropathing.ErrorCalculator;
1915
import com.pedropathing.follower.Follower;
2016
import com.pedropathing.geometry.*;
2117
import com.pedropathing.math.*;
@@ -24,6 +20,7 @@
2420
import com.pedropathing.util.*;
2521
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
2622
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
23+
import com.qualcomm.robotcore.util.ElapsedTime;
2724

2825
import java.util.ArrayList;
2926
import java.util.List;
@@ -52,6 +49,7 @@ public Tuning() {
5249
super("Select a Tuning OpMode", s -> {
5350
s.folder("Localization", l -> {
5451
l.add("Localization Test", LocalizationTest::new);
52+
l.add("Offsets Tuner", OffsetsTuner::new);
5553
l.add("Forward Tuner", ForwardTuner::new);
5654
l.add("Lateral Tuner", LateralTuner::new);
5755
l.add("Turn Tuner", TurnTuner::new);
@@ -61,6 +59,7 @@ public Tuning() {
6159
a.add("Lateral Velocity Tuner", LateralVelocityTuner::new);
6260
a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new);
6361
a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new);
62+
a.add("Predictive Braking", PredictiveBrakingTuner::new);
6463
});
6564
s.folder("Manual", p -> {
6665
p.add("Translational Tuner", TranslationalTuner::new);
@@ -1212,6 +1211,233 @@ public void loop() {
12121211
}
12131212
}
12141213

1214+
/**
1215+
* This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees,
1216+
* and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets
1217+
* are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants.
1218+
*
1219+
* @author Havish Sripada - 12808 RevAmped Robotics
1220+
* @author Baron Henderson
1221+
*/
1222+
class OffsetsTuner extends OpMode {
1223+
@Override
1224+
public void init() {
1225+
follower.setStartingPose(new Pose(72,72));
1226+
follower.update();
1227+
drawOnlyCurrent();
1228+
}
1229+
1230+
/** This initializes the PoseUpdater as well as the Panels telemetry. */
1231+
@Override
1232+
public void init_loop() {
1233+
telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants.");
1234+
telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry.");
1235+
telemetryM.update(telemetry);
1236+
1237+
drawOnlyCurrent();
1238+
}
1239+
1240+
/**
1241+
* This updates the robot's pose estimate, and updates the Panels telemetry with the
1242+
* calculated offsets and draws the robot.
1243+
*/
1244+
@Override
1245+
public void loop() {
1246+
follower.update();
1247+
1248+
telemetryM.debug("Total Angle: " + follower.getTotalHeading());
1249+
1250+
telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer.");
1251+
telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0));
1252+
telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0));
1253+
telemetryM.update(telemetry);
1254+
1255+
draw();
1256+
}
1257+
}
1258+
1259+
/**
1260+
* This is the Predictive Braking Tuner. It runs the robot forward and backward at various power
1261+
* levels, recording the robot’s velocity and position immediately before braking. The motors are
1262+
* then set to zero-power brake mode, which represents the fastest theoretical braking the robot
1263+
* can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance.
1264+
* Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a
1265+
* quadratic curve to model the braking behavior.
1266+
*
1267+
* @author Ashay Sarda - 19745 Turtle Walkers
1268+
* @author Jacob Ophoven - 18535 Frozen Code
1269+
* @version 1.0, 12/26/2025
1270+
*/
1271+
class PredictiveBrakingTuner extends OpMode {
1272+
private static final double[] TEST_POWERS =
1273+
{1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2};
1274+
1275+
private static final int DRIVE_TIME_MS = 1000;
1276+
private static final int BRAKE_WAIT_MS = 500;
1277+
1278+
private enum State {
1279+
START_MOVE,
1280+
WAIT_DRIVE_TIME,
1281+
APPLY_BRAKE,
1282+
WAIT_BRAKE_TIME,
1283+
RECORD,
1284+
DONE
1285+
}
1286+
1287+
private static class BrakeRecord {
1288+
double timeMs;
1289+
Pose pose;
1290+
double velocity;
1291+
1292+
BrakeRecord(double timeMs, Pose pose, double velocity) {
1293+
this.timeMs = timeMs;
1294+
this.pose = pose;
1295+
this.velocity = velocity;
1296+
}
1297+
}
1298+
1299+
private State state = State.START_MOVE;
1300+
1301+
private final ElapsedTime timer = new ElapsedTime();
1302+
1303+
private int iteration = 0;
1304+
1305+
private Vector startPosition;
1306+
private double measuredVelocity;
1307+
1308+
private final List<double[]> velocityToBrakingDistance = new ArrayList<>();
1309+
private final List<BrakeRecord> brakeData = new ArrayList<>();
1310+
1311+
@Override
1312+
public void init() {}
1313+
1314+
@Override
1315+
public void init_loop() {
1316+
telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down.");
1317+
telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet.");
1318+
telemetryM.debug("After stopping, kFriction and kBraking will be displayed.");
1319+
telemetryM.debug("Make sure to turn the timer off.");
1320+
telemetryM.debug("Press B on game pad 1 to stop.");
1321+
telemetryM.update(telemetry);
1322+
follower.update();
1323+
drawOnlyCurrent();
1324+
}
1325+
1326+
@Override
1327+
public void start() {
1328+
timer.reset();
1329+
follower.update();
1330+
follower.startTeleOpDrive(true);
1331+
}
1332+
1333+
@SuppressLint("DefaultLocale")
1334+
@Override
1335+
public void loop() {
1336+
follower.update();
1337+
1338+
if (gamepad1.b) {
1339+
stopRobot();
1340+
requestOpModeStop();
1341+
return;
1342+
}
1343+
1344+
switch (state) {
1345+
case START_MOVE: {
1346+
if (iteration >= TEST_POWERS.length) {
1347+
state = State.DONE;
1348+
break;
1349+
}
1350+
1351+
double currentPower = TEST_POWERS[iteration];
1352+
follower.setMaxPower(currentPower);
1353+
if (iteration % 2 != 0) {
1354+
follower.setTeleOpDrive(-1, 0, 0, true);
1355+
} else {
1356+
follower.setTeleOpDrive(1, 0, 0, true);
1357+
}
1358+
1359+
timer.reset();
1360+
state = State.WAIT_DRIVE_TIME;
1361+
break;
1362+
}
1363+
1364+
case WAIT_DRIVE_TIME: {
1365+
if (timer.milliseconds() >= DRIVE_TIME_MS) {
1366+
measuredVelocity = follower.getVelocity().getMagnitude();
1367+
startPosition = follower.getPose().getAsVector();
1368+
state = State.APPLY_BRAKE;
1369+
}
1370+
break;
1371+
}
1372+
1373+
case APPLY_BRAKE: {
1374+
stopRobot();
1375+
1376+
timer.reset();
1377+
state = State.WAIT_BRAKE_TIME;
1378+
break;
1379+
}
1380+
1381+
case WAIT_BRAKE_TIME: {
1382+
double t = timer.milliseconds();
1383+
Pose currentPose = follower.getPose();
1384+
double currentVelocity = follower.getVelocity().getMagnitude();
1385+
1386+
brakeData.add(new BrakeRecord(t, currentPose, currentVelocity));
1387+
1388+
if (timer.milliseconds() >= BRAKE_WAIT_MS || follower.getVelocity().getMagnitude() <= .05) {
1389+
state = State.RECORD;
1390+
}
1391+
break;
1392+
}
1393+
1394+
case RECORD: {
1395+
Vector endPosition = follower.getPose().getAsVector();
1396+
double brakingDistance = endPosition.minus(startPosition).getMagnitude();
1397+
1398+
velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance});
1399+
1400+
telemetryM.debug("Test " + iteration,
1401+
String.format("v=%.3f d=%.3f", measuredVelocity,
1402+
brakingDistance));
1403+
telemetryM.update(telemetry);
1404+
1405+
iteration++;
1406+
state = State.START_MOVE;
1407+
1408+
break;
1409+
}
1410+
1411+
case DONE: {
1412+
stopRobot();
1413+
1414+
double[] coefficients = quadraticFit(velocityToBrakingDistance);
1415+
1416+
telemetryM.debug("Tuning Complete");
1417+
telemetryM.debug("Braking Profile:");
1418+
telemetryM.debug("kQuadratic", coefficients[1]);
1419+
telemetryM.debug("kLinear", coefficients[0]);
1420+
telemetryM.update(telemetry);
1421+
telemetryM.debug("Tuning Complete");
1422+
telemetryM.debug("Braking Profile:");
1423+
telemetryM.debug("kQuadraticFriction", coefficients[1]);
1424+
telemetryM.debug("kLinearBraking", coefficients[0]);
1425+
for (BrakeRecord record : brakeData) {
1426+
Pose p = record.pose;
1427+
telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f",
1428+
record.timeMs, p.getX(), p.getY(),
1429+
p.getHeading(),
1430+
record.velocity));
1431+
}
1432+
telemetryM.update();
1433+
break;
1434+
}
1435+
}
1436+
1437+
telemetry.update();
1438+
}
1439+
}
1440+
12151441
/**
12161442
* This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot.
12171443
*

‎build.dependencies.gradle‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,8 @@ dependencies {
1515
implementation 'org.firstinspires.ftc:Vision:11.1.0'
1616
implementation 'androidx.appcompat:appcompat:1.2.0'
1717

18-
implementation 'com.pedropathing:ftc:2.0.6'
18+
implementation 'com.pedropathing:ftc:2.1.0'
1919
implementation 'com.pedropathing:telemetry:1.0.0'
20-
implementation 'com.bylazar:fullpanels:1.0.9'
20+
implementation 'com.bylazar:fullpanels:1.0.12'
2121
}
2222

0 commit comments

Comments
 (0)