From dc71eb43179f237584f3a12d04e91f10d7895913 Mon Sep 17 00:00:00 2001
From: Carlos <carlos@riv.as>
Date: Wed, 13 Nov 2024 15:09:59 -0800
Subject: [PATCH] Feature where driver can override centricity (robot vs
 field).

---
 .../teamcode/cometbots/CometBotAutoDevelopment.java | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
index 9ffee06..43954eb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
@@ -34,7 +34,9 @@ public class CometBotAutoDevelopment {
     public Gamepad currentGP2;
     public Gamepad previousGP2;
     private Telemetry telemetry;
+
     public FieldStates fieldStates;
+    private boolean centricity = false;
 
     private Follower follower;
     private HardwareMap hardwareMap;
@@ -99,12 +101,21 @@ public class CometBotAutoDevelopment {
 
         this.toFixMotorBlockingIssueFirstMethod();
         this.toFixMotorBlockingIssueSecondMethod();
+        this.changeCentricity();
 
-        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, false);
+        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity);
         follower.update();
         this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
     }
 
+    public void changeCentricity() {
+        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+            this.centricity = !centricity;
+            this.follower.breakFollowing();
+            this.follower.startTeleopDrive();
+        }
+    }
+
     public void toFixMotorBlockingIssueFirstMethod() {
         if (this.currentGP1.cross && !this.previousGP1.cross) {
             fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);