diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TeleOpFieldCentric.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TeleOpFieldCentric.java new file mode 100644 index 00000000..70fda57e --- /dev/null +++ b/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. + *
+ * 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(); + } + } +} \ No newline at end of file