KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

pedropathing added: tuning progress: forward and lateral velocities done

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-07 20:55:53 -05:00
parent 8f66ddc4bd
commit e40695b4f6
4 changed files with 581 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java
View File

@@ -1,19 +0,0 @@
package org.firstinspires.ftc.teamcode.libs.pedroPathing;
import com.pedropathing.follower.Follower;
import com.pedropathing.follower.FollowerConstants;
import com.pedropathing.ftc.FollowerBuilder;
import com.pedropathing.paths.PathConstraints;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class Constants {
public static FollowerConstants followerConstants = new FollowerConstants();
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
public static Follower createFollower(HardwareMap hardwareMap) {
return new FollowerBuilder(followerConstants, hardwareMap)
.pathConstraints(pathConstraints)
.build();
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java Normal file
View File

@@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode.pedroPathing;
import com.pedropathing.follower.Follower;
import com.pedropathing.follower.FollowerConstants;
import com.pedropathing.ftc.FollowerBuilder;
import com.pedropathing.ftc.drivetrains.MecanumConstants;
import com.pedropathing.ftc.localization.constants.PinpointConstants;
import com.pedropathing.paths.PathConstraints;
import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class Constants {
public static FollowerConstants followerConstants = new FollowerConstants()
.mass(15.5);
public static MecanumConstants driveConstants = new MecanumConstants()
.maxPower(1)
.rightFrontMotorName("fr")
.rightRearMotorName("br")
.leftRearMotorName("bl")
.leftFrontMotorName("fl")
.leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE)
.leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
.rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
.rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
.xVelocity(64.675)
.yVelocity(49.583);
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
public static PinpointConstants localizerConstants = new PinpointConstants()
.forwardPodY(7.5)
.strafePodX(3.75)
.distanceUnit(DistanceUnit.INCH)
.hardwareMapName("pinpoint")
.encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
.forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED)
.strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
public static Follower createFollower(HardwareMap hardwareMap) {
return new FollowerBuilder(followerConstants, hardwareMap)
.pathConstraints(pathConstraints)
.mecanumDrivetrain(driveConstants)
.pinpointLocalizer(localizerConstants)
.build();
}
}

585
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View File

@@ -1,12 +1,14 @@
package org.firstinspires.ftc.teamcode.libs.pedroPathing;
package org.firstinspires.ftc.teamcode.pedroPathing;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.changes;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrent;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrentAndHistory;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.telemetryM;
import static com.pedropathing.math.MathFunctions.quadraticFit;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrent;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrentAndHistory;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
import android.annotation.SuppressLint;
import com.bylazar.configurables.PanelsConfigurables;
import com.bylazar.configurables.annotations.Configurable;
@@ -17,13 +19,20 @@ import com.bylazar.field.Style;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.*;
import com.pedropathing.math.*;
import com.pedropathing.paths.*;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.math.Vector;
import com.pedropathing.paths.HeadingInterpolator;
import com.pedropathing.paths.Path;
import com.pedropathing.paths.PathChain;
import com.pedropathing.telemetry.SelectableOpMode;
import com.pedropathing.util.*;
import com.pedropathing.util.PoseHistory;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.util.ElapsedTime;
import java.util.ArrayList;
import java.util.List;
@@ -52,6 +61,7 @@ public class Tuning extends SelectableOpMode {
super("Select a Tuning OpMode", s -> {
s.folder("Localization", l -> {
l.add("Localization Test", LocalizationTest::new);
l.add("Offsets Tuner", OffsetsTuner::new);
l.add("Forward Tuner", ForwardTuner::new);
l.add("Lateral Tuner", LateralTuner::new);
l.add("Turn Tuner", TurnTuner::new);
@@ -61,6 +71,7 @@ public class Tuning extends SelectableOpMode {
a.add("Lateral Velocity Tuner", LateralVelocityTuner::new);
a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new);
a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new);
a.add("Predictive Braking Tuner", PredictiveBrakingTuner::new);
});
s.folder("Manual", p -> {
p.add("Translational Tuner", TranslationalTuner::new);
@@ -73,6 +84,11 @@ public class Tuning extends SelectableOpMode {
p.add("Triangle", Triangle::new);
p.add("Circle", Circle::new);
});
s.folder("Swerve", p-> {
p.add("Analog Min / Max Tuner", AnalogMinMaxTuner::new);
p.add("Swerve Offsets Test", SwerveOffsetsTest::new);
p.add("Swerve Turn Test", SwerveTurnTest::new);
});
});
}
@@ -117,23 +133,35 @@ public class Tuning extends SelectableOpMode {
}
/**
* This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a
* This is the LocalizationTest OpMode. This is basically just a simple drive attached to a
* PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot.
* You should use this to check the robot's localization.
*
* @author Anyi Lin - 10158 Scott's Bots
* @author Baron Henderson - 20077 The Indubitables
* @author Kabir Goyal
* @version 1.0, 5/6/2024
*/
class LocalizationTest extends OpMode {
@Override
public void init() {}
boolean debugStringEnabled = false;
/** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */
@Override
public void init() {
follower.setStartingPose(new Pose(72,72));
}
/** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */
@Override
public void init_loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
telemetryM.debug("This will print your robot's position to telemetry while "
+ "allowing robot control through a basic mecanum drive on gamepad 1.");
+ "allowing robot control through a basic drive on gamepad 1.");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)");
telemetryM.update(telemetry);
follower.update();
drawCurrent();
@@ -146,11 +174,15 @@ class LocalizationTest extends OpMode {
}
/**
* This updates the robot's pose estimate, the simple mecanum drive, and updates the
* This updates the robot's pose estimate, the simple drive, and updates the
* Panels telemetry with the robot's position as well as draws the robot's position.
*/
@Override
public void loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true);
follower.update();
@@ -158,6 +190,10 @@ class LocalizationTest extends OpMode {
telemetryM.debug("y:" + follower.getPose().getY());
telemetryM.debug("heading:" + follower.getPose().getHeading());
telemetryM.debug("total heading:" + follower.getTotalHeading());
if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString());
}
telemetryM.update(telemetry);
drawCurrentAndHistory();
@@ -182,6 +218,7 @@ class ForwardTuner extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72,72));
follower.update();
drawCurrent();
}
@@ -229,6 +266,7 @@ class LateralTuner extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72,72));
follower.update();
drawCurrent();
}
@@ -276,6 +314,7 @@ class TurnTuner extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72,72));
follower.update();
drawCurrent();
}
@@ -312,7 +351,7 @@ class TurnTuner extends OpMode {
* reaching the end of the distance, it averages them and prints out the velocity obtained. It is
* recommended to run this multiple times on a full battery to get the best results. What this does
* is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that
* empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
* empirically represents the direction your wheels actually prefer to go in, allowing for
* more accurate following.
*
* @author Anyi Lin - 10158 Scott's Bots
@@ -323,13 +362,15 @@ class TurnTuner extends OpMode {
*/
class ForwardVelocityTuner extends OpMode {
private final ArrayList<Double> velocities = new ArrayList<>();
public static double DISTANCE = 48;
public static double DISTANCE = 120;
public static double RECORD_NUMBER = 10;
private boolean end;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */
@Override
@@ -374,7 +415,7 @@ class ForwardVelocityTuner extends OpMode {
if (!end) {
if (Math.abs(follower.getPose().getX()) > DISTANCE) {
if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) {
end = true;
stopRobot();
} else {
@@ -417,7 +458,7 @@ class ForwardVelocityTuner extends OpMode {
* reaching the end of the distance, it averages them and prints out the velocity obtained. It is
* recommended to run this multiple times on a full battery to get the best results. What this does
* is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that
* empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
* empirically represents the direction your wheels actually prefer to go in, allowing for
* more accurate following.
*
* @author Anyi Lin - 10158 Scott's Bots
@@ -429,13 +470,15 @@ class ForwardVelocityTuner extends OpMode {
class LateralVelocityTuner extends OpMode {
private final ArrayList<Double> velocities = new ArrayList<>();
public static double DISTANCE = 48;
public static double DISTANCE = 120;
public static double RECORD_NUMBER = 10;
private boolean end;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/**
* This initializes the drive motors as well as the cache of velocities and the Panels
@@ -480,7 +523,7 @@ class LateralVelocityTuner extends OpMode {
drawCurrentAndHistory();
if (!end) {
if (Math.abs(follower.getPose().getY()) > DISTANCE) {
if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) {
end = true;
stopRobot();
} else {
@@ -537,7 +580,9 @@ class ForwardZeroPowerAccelerationTuner extends OpMode {
private boolean end;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/** This initializes the drive motors as well as the Panels telemetryM. */
@Override
@@ -639,7 +684,9 @@ class LateralZeroPowerAccelerationTuner extends OpMode {
private boolean end;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/** This initializes the drive motors as well as the Panels telemetry. */
@Override
@@ -717,6 +764,185 @@ class LateralZeroPowerAccelerationTuner extends OpMode {
}
}
/**
* 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 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.
*
* @author Ashay Sarda - 19745 Turtle Walkers
* @author Jacob Ophoven - 18535 Frozen Code
* @version 1.0, 12/26/2025
*/
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 double BRAKING_POWER = -0.2;
private static final int DRIVE_TIME_MS = 1000;
private enum State {
START_MOVE,
WAIT_DRIVE_TIME,
APPLY_BRAKE,
WAIT_BRAKE_TIME,
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 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.");
telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet.");
telemetryM.debug("After stopping, kFriction and kBraking will be displayed.");
telemetryM.debug("Make sure to turn the timer off.");
telemetryM.debug("Press B on game pad 1 to stop.");
telemetryM.update(telemetry);
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);
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();
startPosition = follower.getPose().getAsVector();
state = State.APPLY_BRAKE;
}
break;
}
case APPLY_BRAKE: {
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 (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("kQuadratic", coefficients[1]);
telemetryM.debug("kLinear", coefficients[0]);
telemetryM.update(telemetry);
telemetryM.debug("Tuning Complete");
telemetryM.debug("Braking Profile:");
telemetryM.debug("kQuadraticFriction", coefficients[1]);
telemetryM.debug("kLinearBraking", coefficients[0]);
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));
}
telemetryM.update();
break;
}
}
}
}
/**
* This is the Translational PIDF Tuner OpMode. It will keep the robot in place.
* The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly.
@@ -735,7 +961,9 @@ class TranslationalTuner extends OpMode {
private Path backwards;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/** This initializes the Follower and creates the forward and backward Paths. */
@Override
@@ -752,9 +980,9 @@ class TranslationalTuner extends OpMode {
public void start() {
follower.deactivateAllPIDFs();
follower.activateTranslational();
forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
}
@@ -776,6 +1004,9 @@ class TranslationalTuner extends OpMode {
}
telemetryM.debug("Push the robot laterally to test the Translational PIDF(s).");
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent());
telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent());
telemetryM.update(telemetry);
}
}
@@ -799,7 +1030,9 @@ class HeadingTuner extends OpMode {
private Path backwards;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/**
* This initializes the Follower and creates the forward and backward Paths. Additionally, this
@@ -819,9 +1052,9 @@ class HeadingTuner extends OpMode {
public void start() {
follower.deactivateAllPIDFs();
follower.activateHeading();
forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
}
@@ -846,6 +1079,8 @@ class HeadingTuner extends OpMode {
}
telemetryM.debug("Turn the robot manually to test the Heading PIDF(s).");
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getHeadingError());
telemetryM.update(telemetry);
}
}
@@ -867,7 +1102,9 @@ class DriveTuner extends OpMode {
private PathChain backwards;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/**
* This initializes the Follower and creates the forward and backward Paths. Additionally, this
@@ -890,13 +1127,13 @@ class DriveTuner extends OpMode {
forwards = follower.pathBuilder()
.setGlobalDeceleration()
.addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)))
.addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)))
.setConstantHeadingInterpolation(0)
.build();
backwards = follower.pathBuilder()
.setGlobalDeceleration()
.addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)))
.addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)))
.setConstantHeadingInterpolation(0)
.build();
@@ -923,6 +1160,8 @@ class DriveTuner extends OpMode {
}
telemetryM.debug("Driving forward?: " + forward);
telemetryM.addData("Zero Line", 0);
telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]);
telemetryM.update(telemetry);
}
}
@@ -945,7 +1184,9 @@ class Line extends OpMode {
private Path backwards;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/** This initializes the Follower and creates the forward and backward Paths. */
@Override
@@ -961,9 +1202,9 @@ class Line extends OpMode {
@Override
public void start() {
follower.activateAllPIDFs();
forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
forwards.setConstantHeadingInterpolation(0);
backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
backwards.setConstantHeadingInterpolation(0);
follower.followPath(forwards);
}
@@ -1010,7 +1251,9 @@ class CentripetalTuner extends OpMode {
private Path backwards;
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/**
* This initializes the Follower and creates the forward and backward Paths.
@@ -1029,8 +1272,8 @@ class CentripetalTuner extends OpMode {
@Override
public void start() {
follower.activateAllPIDFs();
forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE)));
backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0)));
forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72)));
backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72)));
backwards.setTangentHeadingInterpolation();
backwards.reverseHeadingInterpolation();
@@ -1071,9 +1314,9 @@ class CentripetalTuner extends OpMode {
*/
class Triangle extends OpMode {
private final Pose startPose = new Pose(0, 0, Math.toRadians(0));
private final Pose interPose = new Pose(24, -24, Math.toRadians(90));
private final Pose endPose = new Pose(24, 24, Math.toRadians(45));
private final Pose startPose = new Pose(72, 72, Math.toRadians(0));
private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90));
private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45));
private PathChain triangle;
@@ -1092,7 +1335,9 @@ class Triangle extends OpMode {
}
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
@Override
public void init_loop() {
@@ -1138,14 +1383,14 @@ class Circle extends OpMode {
public void start() {
circle = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
.addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
.addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
.addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
.addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72)))
.setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
.build();
follower.followPath(circle);
}
@@ -1161,7 +1406,9 @@ class Circle extends OpMode {
}
@Override
public void init() {}
public void init() {
follower.setStartingPose(new Pose(72, 72));
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
@@ -1178,6 +1425,230 @@ class Circle extends OpMode {
}
}
/**
* Tuning OpMode to get the min and max encoder values for swerve pods
* @author Kabir Goyal
*/
class AnalogMinMaxTuner extends OpMode {
//populate the below with your names for the servos and encoders
public String[] encoderNames = {"leftFrontEncoder", "rightFrontEncoder", "leftBackEncoder", "rightBackEncoder"};
public AnalogInput[] encoders = new AnalogInput[encoderNames.length];
public double[] minVoltages = new double[encoderNames.length];
public double[] maxVoltages = new double[encoderNames.length];
public List<LynxModule> lynxModules; //js to improve loop times a bit yk
public void start() {
}
@Override
public void init_loop() {
telemetryM.debug("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" +
"The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.");
telemetryM.update(telemetry);
}
@Override
public void init() {
lynxModules = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : lynxModules) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
for (int i = 0; i < encoders.length; i++) {
encoders[i] = hardwareMap.get(AnalogInput.class, encoderNames[i]);
minVoltages[i] = 5; //bigger value than should ever be read
}
}
/**
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
* the Telemetry, as well as the FTC Dashboard.
*/
@Override
public void loop() {
for (LynxModule hub : lynxModules) {
hub.clearBulkCache();
}
telemetryM.debug("Spin each pod slowly for 4 to 5 full rotations.\n" +
"The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n");
for (int i = 0; i < encoders.length; i++) {
double currentVoltage = encoders[i].getVoltage();
minVoltages[i] = Math.min(minVoltages[i], currentVoltage);
maxVoltages[i] = Math.max(maxVoltages[i], currentVoltage);
telemetryM.addData(encoderNames[i] + "min value:", minVoltages[i]);
telemetryM.addData(encoderNames[i] + "max value:", maxVoltages[i]);
telemetryM.addLine("");
}
telemetryM.update();
}
}
/**
* This is the SwerveOffsetsTest
* You should use this to check how good your swerve angle offsets are and if your motor directions are correct
* @author Kabir Goyal
*
*/
class SwerveOffsetsTest extends OpMode {
boolean debugStringEnabled = false;
@Override
public void init() {}
/** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */
@Override
public void init_loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
telemetryM.debug("This OpMode will run all four swerve pods in the direction they think is forward"
+ "\nensure your bot is not on the ground while running");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)");
telemetryM.update(telemetry);
follower.update();
drawCurrent();
}
@Override
public void start() {
follower.startTeleopDrive();
follower.update();
}
/**
* This updates the robot's pose estimate, the simple drive, and updates the
* Panels telemetry with the robot's position as well as draws the robot's position.
*/
@Override
public void loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
follower.setTeleOpDrive(0.25, 0, 0, true);
follower.update();
if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString());
}
telemetryM.update(telemetry);
drawCurrentAndHistory();
}
}
/**
* This is the SwerveTurnTest
* You should use this to check your encoder directions and x/y pod offsets
* @author Kabir Goyal
*
*/
class SwerveTurnTest extends OpMode {
boolean debugStringEnabled = false;
@Override
public void init() {}
/** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */
@Override
public void init_loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
telemetryM.debug("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) "
+ "\nrun this once off the ground to check servo directions and motor directions before testing on the ground");
telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) +
" (press gamepad a to toggle)");
telemetryM.update(telemetry);
follower.update();
drawCurrent();
}
@Override
public void start() {
follower.startTeleopDrive();
follower.update();
}
/**
* This updates the robot's pose estimate, the simple drive, and updates the
* Panels telemetry with the robot's position as well as draws the robot's position.
*/
@Override
public void loop() {
if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) {
debugStringEnabled = !debugStringEnabled;
}
follower.setTeleOpDrive(0, 0, 0.25, true);
follower.update();
if (debugStringEnabled) {
telemetryM.debug("Drivetrain Debug String:\n" +
follower.getDrivetrain().debugString());
}
telemetryM.update(telemetry);
drawCurrentAndHistory();
}
}
/**
* This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees,
* and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets
* are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants.
*
* @author Havish Sripada - 12808 RevAmped Robotics
* @author Baron Henderson
*/
class OffsetsTuner extends OpMode {
@Override
public void init() {
follower.setStartingPose(new Pose(72,72));
follower.update();
drawCurrent();
}
/** This initializes the PoseUpdater as well as the Panels telemetry. */
@Override
public void init_loop() {
telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants.");
telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry.");
telemetryM.update(telemetry);
drawCurrent();
}
/**
* This updates the robot's pose estimate, and updates the Panels telemetry with the
* calculated offsets and draws the robot.
*/
@Override
public void loop() {
follower.update();
telemetryM.debug("Total Angle: " + follower.getTotalHeading());
telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer.");
telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0));
telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0));
telemetryM.update(telemetry);
drawCurrentAndHistory();
}
}
/**
* This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot.
*
@@ -1189,10 +1660,10 @@ class Drawing {
private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
private static final Style robotLook = new Style(
"", "#3F51B5", 0.0
"", "#3F51B5", 0.75
);
private static final Style historyLook = new Style(
"", "#4CAF50", 0.0
"", "#4CAF50", 0.75
);
/**