diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index cdb154f38a2e..a825da392e6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -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; @@ -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); + }); }); } @@ -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(); @@ -152,11 +170,15 @@ 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(); @@ -164,6 +186,10 @@ public void loop() { 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(); @@ -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 @@ -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 @@ -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 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. *