Merge pull request #68 from MOEbo-Sapiens/dev

Swerve Test Opmodes

Author: junkjunk123

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,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();
@@ -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<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.
  *