field centric teleop

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TeleOpFieldCentric.java

@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.robot;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+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.robot.CenterStageTestRobot.Drive;
+
+/**
+ * This opmode demonstrates how one would implement field centric control using
+ * `SampleMecanumDrive.java`. This file is essentially just `TeleOpDrive.java` with the addition of
+ * field centric control. To achieve field centric control, the only modification one needs is to
+ * rotate the input vector by the current heading before passing it into the inverse kinematics.
+ * <p>
+ * See lines 42-57.
+ */
+@TeleOp(group = "advanced")
+public class TeleOpFieldCentric extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        // Initialize SampleMecanumDrive
+        Drive drive = new Drive(hardwareMap);
+
+        // We want to turn off velocity control for teleop
+        // Velocity control per wheel is not necessary outside of motion profiled auto
+        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        // Retrieve our pose from the PoseStorage.currentPose static field
+        // See AutoTransferPose.java for further details
+        drive.setPoseEstimate(PoseStorage.currentPose);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive() && !isStopRequested()) {
+            // Read pose
+            Pose2d poseEstimate = drive.getPoseEstimate();
+
+            // Create a vector from the gamepad x/y inputs
+            // Then, rotate that vector by the inverse of that heading
+            Vector2d input = new Vector2d(
+                    -gamepad1.left_stick_y,
+                    -gamepad1.left_stick_x
+            ).rotated(-poseEstimate.getHeading());
+
+            // Pass in the rotated input + right stick value for rotation
+            // Rotation is not part of the rotated input thus must be passed in separately
+            drive.setWeightedDrivePower(
+                    new Pose2d(
+                            input.getX(),
+                            input.getY(),
+                            -gamepad1.right_stick_x
+                    )
+            );
+
+            // Update everything. Odometry. Etc.
+            drive.update();
+
+            // Print pose to telemetry
+            telemetry.addData("x", poseEstimate.getX());
+            telemetry.addData("y", poseEstimate.getY());
+            telemetry.addData("heading", poseEstimate.getHeading());
+            telemetry.update();
+        }
+    }
+}