SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add Roadrunner

Browse Source
This commit is contained in:
Nick Haemel
2023-08-28 21:16:35 -07:00
parent 729ba8a969
commit 05e56926b2
44 changed files with 4337 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

221
TeamCode/drive/opmode/AutomaticFeedforwardTuner.java Normal file
View File

@ -0,0 +1,221 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.util.LoggingUtil;
import org.firstinspires.ftc.teamcode.util.RegressionUtil;
import java.util.ArrayList;
import java.util.List;
/*
* Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
* outline of the procedure:
* 1. Slowly ramp the motor power and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
* and an optional intercept (kStatic).
* 3. Accelerate the robot (apply constant power) and record the encoder counts.
* 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
* regression.
*/
@Config
@Autonomous(group = "drive")
public class AutomaticFeedforwardTuner extends LinearOpMode {
public static double MAX_POWER = 0.7;
public static double DISTANCE = 100; // in
@Override
public void runOpMode() throws InterruptedException {
if (RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
"when using the built-in drive motor velocity PID.");
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
NanoClock clock = NanoClock.system();
telemetry.addLine("Press play to begin the feedforward tuning routine");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Would you like to fit kStatic?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitIntercept = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitIntercept = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
telemetry.clearAll();
telemetry.addLine(Misc.formatInvariant(
"Place your robot on the field with at least %.2f in of room in front", DISTANCE));
telemetry.addLine("Press (Y/Δ) to begin");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxVel = rpmToVelocity(MAX_RPM);
double finalVel = MAX_POWER * maxVel;
double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
List<Double> timeSamples = new ArrayList<>();
List<Double> positionSamples = new ArrayList<>();
List<Double> powerSamples = new ArrayList<>();
drive.setPoseEstimate(new Pose2d());
double startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > rampTime) {
break;
}
double vel = accel * elapsedTime;
double power = vel / maxVel;
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(power);
drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
timeSamples, positionSamples, powerSamples, fitIntercept,
LoggingUtil.getLogFile(Misc.formatInvariant(
"DriveRampRegression-%d.csv", System.currentTimeMillis())));
telemetry.clearAll();
telemetry.addLine("Quasi-static ramp up test complete");
if (fitIntercept) {
telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
rampResult.kV, rampResult.kStatic, rampResult.rSquare));
} else {
telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
rampResult.kStatic, rampResult.rSquare));
}
telemetry.addLine("Would you like to fit kA?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitAccelFF = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitAccelFF = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
if (fitAccelFF) {
telemetry.clearAll();
telemetry.addLine("Place the robot back in its starting position");
telemetry.addLine("Press (Y/Δ) to continue");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxPowerTime = DISTANCE / maxVel;
timeSamples.clear();
positionSamples.clear();
powerSamples.clear();
drive.setPoseEstimate(new Pose2d());
drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > maxPowerTime) {
break;
}
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(MAX_POWER);
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
timeSamples, positionSamples, powerSamples, rampResult,
LoggingUtil.getLogFile(Misc.formatInvariant(
"DriveAccelRegression-%d.csv", System.currentTimeMillis())));
telemetry.clearAll();
telemetry.addLine("Constant power test complete");
telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
accelResult.kA, accelResult.rSquare));
telemetry.update();
}
while (!isStopRequested()) {
idle();
}
}
}

52
TeamCode/drive/opmode/BackAndForth.java Normal file
View File

@ -0,0 +1,52 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin moving forward and
* backward. You should observe the target position (green) and your pose estimate (blue) and adjust
* your follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*
* This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
* is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
*/
@Config
@Autonomous(group = "drive")
public class BackAndForth extends LinearOpMode {
public static double DISTANCE = 50;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
.back(DISTANCE)
.build();
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
drive.followTrajectory(trajectoryForward);
drive.followTrajectory(trajectoryBackward);
}
}
}

171
TeamCode/drive/opmode/DriveVelocityPIDTuner.java Normal file
View File

@ -0,0 +1,171 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import java.util.List;
/*
* This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
* loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
* important as the positional parameters. Like the other manual tuning routines, this op mode
* relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
* WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
* phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
* connected, start the program, and your robot will begin moving forward and backward according to
* a motion profile. Your job is to graph the velocity errors over time and adjust the PID
* coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
* Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
* MOTOR_VELO_PID field.
*
* Recommended tuning process:
*
* 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
* mitigate oscillations.
* 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
* 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
@Config
@Autonomous(group = "drive")
public class DriveVelocityPIDTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
if (!RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
"PID is not in use", getClass().getSimpleName());
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Mode mode = Mode.TUNING_MODE;
double lastKp = MOTOR_VELO_PID.p;
double lastKi = MOTOR_VELO_PID.i;
double lastKd = MOTOR_VELO_PID.d;
double lastKf = MOTOR_VELO_PID.f;
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = kV * motionState.getV();
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
List<Double> velocities = drive.getWheelVelocities();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
for (int i = 0; i < velocities.size(); i++) {
telemetry.addData("measuredVelocity" + i, velocities.get(i));
telemetry.addData(
"error" + i,
motionState.getV() - velocities.get(i)
);
}
break;
case DRIVER_MODE:
if (gamepad1.b) {
drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
|| lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
lastKp = MOTOR_VELO_PID.p;
lastKi = MOTOR_VELO_PID.i;
lastKd = MOTOR_VELO_PID.d;
lastKf = MOTOR_VELO_PID.f;
}
telemetry.update();
}
}
}

55
TeamCode/drive/opmode/FollowerPIDTuner.java Normal file
View File

@ -0,0 +1,55 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin driving in a square.
* You should observe the target position (green) and your pose estimate (blue) and adjust your
* follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*/
@Config
@Autonomous(group = "drive")
public class FollowerPIDTuner extends LinearOpMode {
public static double DISTANCE = 48; // in
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
drive.setPoseEstimate(startPose);
waitForStart();
if (isStopRequested()) return;
while (!isStopRequested()) {
TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.build();
drive.followTrajectorySequence(trajSeq);
}
}
}

45
TeamCode/drive/opmode/LocalizationTest.java Normal file
View File

@ -0,0 +1,45 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
* encoder localizer heading may be significantly off if the track width has not been tuned).
*/
@TeleOp(group = "drive")
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
while (!isStopRequested()) {
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
drive.update();
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
telemetry.update();
}
}
}

154
TeamCode/drive/opmode/ManualFeedforwardTuner.java Normal file
View File

@ -0,0 +1,154 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.kinematics.Kinematics;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import java.util.Objects;
/*
* This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
* tuning these coefficients is just as important as the positional parameters. Like the other
* manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
* you are using the Control Hub. Once you've successfully connected, start the program, and your
* robot will begin moving forward and backward according to a motion profile. Your job is to graph
* the velocity errors over time and adjust the feedforward coefficients. Once you've found a
* satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
@Config
@Autonomous(group = "drive")
public class ManualFeedforwardTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
private FtcDashboard dashboard = FtcDashboard.getInstance();
private SampleMecanumDrive drive;
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private Mode mode;
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
if (RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
"when using the built-in drive motor velocity PID.");
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
drive = new SampleMecanumDrive(hardwareMap);
final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next();
mode = Mode.TUNING_MODE;
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
final double NOMINAL_VOLTAGE = 12.0;
final double voltage = voltageSensor.getVoltage();
drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0));
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
double currentVelo = poseVelo.getX();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
telemetry.addData("measuredVelocity", currentVelo);
telemetry.addData("error", motionState.getV() - currentVelo);
break;
case DRIVER_MODE:
if (gamepad1.b) {
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
telemetry.update();
}
}
}

73
TeamCode/drive/opmode/MaxAngularVeloTuner.java Normal file
View File

@ -0,0 +1,73 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
* <p>
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
* <p>
* Further fine tuning of MAX_ANG_VEL may be desired.
*/
@Config
@Autonomous(group = "drive")
public class MaxAngularVeloTuner extends LinearOpMode {
public static double RUNTIME = 4.0;
private ElapsedTime timer;
private double maxAngVelocity = 0.0;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(0, 0, 1));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
}
drive.setDrivePower(new Pose2d());
telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
telemetry.update();
while (!isStopRequested()) idle();
}
}

84
TeamCode/drive/opmode/MaxVelocityTuner.java Normal file
View File

@ -0,0 +1,84 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum velocity your bot can achieve under load. It
* will also calculate the effective kF value for your velocity PID.
* <p>
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
* <p>
* Further fine tuning of kF may be desired.
*/
@Config
@Autonomous(group = "drive")
public class MaxVelocityTuner extends LinearOpMode {
public static double RUNTIME = 2.0;
private ElapsedTime timer;
private double maxVelocity = 0.0;
private VoltageSensor batteryVoltageSensor;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(1, 0, 0));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
}
drive.setDrivePower(new Pose2d());
double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
telemetry.addData("Max Velocity", maxVelocity);
telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
telemetry.update();
while (!isStopRequested() && opModeIsActive()) idle();
}
private double veloInchesToTicks(double inchesPerSec) {
return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
}
}

93
TeamCode/drive/opmode/MotorDirectionDebugger.java Normal file
View File

@ -0,0 +1,93 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for debugging your motor configuration.
* Pressing each of the buttons will power its respective motor.
*
* Button Mappings:
*
* Xbox/PS4 Button - Motor
* X / ▢ - Front Left
* Y / Δ - Front Right
* B / O - Rear Right
* A / X - Rear Left
* The buttons are mapped to match the wheels spatially if you
* were to rotate the gamepad 45deg°. x/square is the front left
* ________ and each button corresponds to the wheel as you go clockwise
* / ______ \
* ------------.-' _ '-..+ Front of Bot
* / _ ( Y ) _ \ ^
* | ( X ) _ ( B ) | Front Left \ Front Right
* ___ '. ( A ) /| Wheel \ Wheel
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
* | | | \
* '.___.' '. | Rear Left \ Rear Right
* '. / Wheel \ Wheel
* \. .' (A/X) \ (B/O)
* \________/
*
* Uncomment the @Disabled tag below to use this opmode.
*/
@Disabled
@Config
@TeleOp(group = "drive")
public class MotorDirectionDebugger extends LinearOpMode {
public static double MOTOR_POWER = 0.7;
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
telemetry.addLine("Press play to begin the debugging opmode");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
while (!isStopRequested()) {
telemetry.addLine("Press each button to turn on its respective motor");
telemetry.addLine();
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
telemetry.addLine();
if(gamepad1.x) {
drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
telemetry.addLine("Running Motor: Front Left");
} else if(gamepad1.y) {
drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
telemetry.addLine("Running Motor: Front Right");
} else if(gamepad1.b) {
drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
telemetry.addLine("Running Motor: Rear Right");
} else if(gamepad1.a) {
drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
telemetry.addLine("Running Motor: Rear Left");
} else {
drive.setMotorPowers(0, 0, 0, 0);
telemetry.addLine("Running Motor: None");
}
telemetry.update();
}
}
}

38
TeamCode/drive/opmode/SplineTest.java Normal file
View File

@ -0,0 +1,38 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* This is an example of a more complex path to really test the tuning.
*/
@Autonomous(group = "drive")
public class SplineTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
Trajectory traj = drive.trajectoryBuilder(new Pose2d())
.splineTo(new Vector2d(30, 30), 0)
.build();
drive.followTrajectory(traj);
sleep(2000);
drive.followTrajectory(
drive.trajectoryBuilder(traj.end(), true)
.splineTo(new Vector2d(0, 0), Math.toRadians(180))
.build()
);
}
}

46
TeamCode/drive/opmode/StrafeTest.java Normal file
View File

@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
public class StrafeTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.strafeRight(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

46
TeamCode/drive/opmode/StraightTest.java Normal file
View File

@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
public class StraightTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

88
TeamCode/drive/opmode/TrackWidthTuner.java Normal file
View File

@ -0,0 +1,88 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* This routine determines the effective track width. The procedure works by executing a point turn
* with a given angle and measuring the difference between that angle and the actual angle (as
* indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
* given angle / actual angle gives a multiplicative adjustment to the estimated track width
* (effective track width = estimated track width * given angle / actual angle). The routine repeats
* this procedure a few times and averages the values for additional accuracy. Note: a relatively
* accurate track width estimate is important or else the angular constraints will be thrown off.
*/
@Config
@Autonomous(group = "drive")
public class TrackWidthTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// TODO: if you haven't already, set the localizer to something that doesn't depend on
// drive encoders for computing the heading
telemetry.addLine("Press play to begin the track width tuner routine");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.normDelta(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
trackWidthStats.add(trackWidth);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
trackWidthStats.getMean(),
trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

104
TeamCode/drive/opmode/TrackingWheelForwardOffsetTuner.java Normal file
View File

@ -0,0 +1,104 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
/**
* This routine determines the effective forward offset for the lateral tracking wheel.
* The procedure executes a point turn at a given angle for a certain number of trials,
* along with a specified delay in milliseconds. The purpose of this is to track the
* change in the y position during the turn. The offset, or distance, of the lateral tracking
* wheel from the center or rotation allows the wheel to spin during a point turn, leading
* to an incorrect measurement for the y position. This creates an arc around around
* the center of rotation with an arc length of change in y and a radius equal to the forward
* offset. We can compute this offset by calculating (change in y position) / (change in heading)
* which returns the radius if the angle (change in heading) is in radians. This is based
* on the arc length formula of length = theta * radius.
*
* To run this routine, simply adjust the desired angle and specify the number of trials
* and the desired delay. Then, run the procedure. Once it finishes, it will print the
* average of all the calculated forward offsets derived from the calculation. This calculated
* forward offset is then added onto the current forward offset to produce an overall estimate
* for the forward offset. You can run this procedure as many times as necessary until a
* satisfactory result is produced.
*/
@Config
@Autonomous(group="drive")
public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Press play to begin the forward offset tuner");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.norm(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
drive.getPoseEstimate().getY() / headingAccumulator;
forwardOffsetStats.add(forwardOffset);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
forwardOffsetStats.getMean(),
forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

130
TeamCode/drive/opmode/TrackingWheelLateralDistanceTuner.java Normal file
View File

@ -0,0 +1,130 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
/**
* Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
* LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
* wheels.
*
* Tuning Routine:
*
* 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
* measured value. This need only be an estimated value as you will be tuning it anyways.
*
* 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
* similar mark right below the indicator on your bot. This will be your reference point to
* ensure you've turned exactly 360°.
*
* 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
* identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
* if you are using the Control Hub.
* Ensure the field is showing (select the field view in top right of the page).
*
* 4. Press play to begin the tuning routine.
*
* 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
*
* 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
*
* 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
* on the bot and on the ground you created earlier should be lined up.
*
* 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
* StandardTrackingWheelLocalizer.java class.
*
* 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
* yourself. Read the heading output and follow the advice stated in the note below to manually
* nudge the values yourself.
*
* Note:
* It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
* a line from the circumference to the center should be present, representing the bot. The line
* indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
* dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
* the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
* actual bot, the LATERAL_DISTANCE should be increased.
*
* If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
* position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
* effective center of rotation. You can ignore this effect. The center of rotation will be offset
* slightly but your heading will still be fine. This does not affect your overall tracking
* precision. The heading should still line up.
*/
@Config
@TeleOp(group = "drive")
public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
public static int NUM_TURNS = 10;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Prior to beginning the routine, please read the directions "
+ "located in the comments of the opmode file.");
telemetry.addLine("Press play to begin the tuning routine.");
telemetry.addLine("");
telemetry.addLine("Press Y/△ to stop the routine.");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.update();
double headingAccumulator = 0;
double lastHeading = 0;
boolean tuningFinished = false;
while (!isStopRequested() && !tuningFinished) {
Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
drive.setDrivePower(vel);
drive.update();
double heading = drive.getPoseEstimate().getHeading();
double deltaHeading = heading - lastHeading;
headingAccumulator += Angle.normDelta(deltaHeading);
lastHeading = heading;
telemetry.clearAll();
telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
telemetry.addLine();
telemetry.addLine("Press Y/△ to conclude routine");
telemetry.update();
if (gamepad1.y)
tuningFinished = true;
}
telemetry.clearAll();
telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
telemetry.addLine("Effective LATERAL_DISTANCE: " +
(headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
telemetry.update();
while (!isStopRequested()) idle();
}
}

27
TeamCode/drive/opmode/TurnTest.java Normal file
View File

@ -0,0 +1,27 @@
package org.firstinspires.ftc.teamcode.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/*
* This is a simple routine to test turning capabilities.
*/
@Config
@Autonomous(group = "drive")
public class TurnTest extends LinearOpMode {
public static double ANGLE = 90; // deg
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
drive.turn(Math.toRadians(ANGLE));
}
}