Skip to content

Swerve Test Opmodes #68

New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Merged
junkjunk123 merged 3 commits into from
Mar 5, 2026
Merged

Swerve Test Opmodes #68

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
892f9ec
add debug string to LocalizationTest for swerve
GoKubar Mar 4, 2026
fd80008
make debug string toggleable in Localization test
GoKubar Mar 4, 2026
9986acb
SwerveOffsetTest and SwerveTurningTest
GoKubar Mar 4, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
217 changes: 211 additions & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,11 @@

import android.annotation.SuppressLint;

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.hardware.CRServo;
import com.qualcomm.robotcore.util.ElapsedTime;

import java.util.ArrayList;
Expand Down Expand Up @@ -79,6 +82,11 @@ public Tuning() {
p.add("Circle", Circle::new);
p.add("Line90DegreeTurn", Line90DegreeTurn::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);
});
});
}

Expand Down Expand Up @@ -123,23 +131,33 @@ public static void stopRobot() {
}

/**
* 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 {
boolean debugStringEnabled = false;

@Override
public void init() {}

/** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */
/** 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();
Expand All @@ -152,18 +170,26 @@ public void start() {
}

/**
* 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();

telemetryM.debug("x:" + follower.getPose().getX());
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();
Expand Down Expand Up @@ -318,7 +344,7 @@ public void loop() {
* 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
Expand Down Expand Up @@ -423,7 +449,7 @@ public void loop() {
* 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
Expand Down Expand Up @@ -1405,6 +1431,185 @@ public void loop() {
}
}

/**
* 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 Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot.
*
Expand Down