Add Roadrunner

TeamCode/drive/opmode/TrackingWheelLateralDistanceTuner.java

@@ -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();
+    }
+}