From 1dc28841f1db6a1b355862098153abed56451b1e Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Sat, 6 Jan 2024 12:47:09 -0600
Subject: [PATCH] Pizza bot - Still working on the field centric drive and arm
 has been commented out.

---
 .../CenterStage/Common/MiniYTeleOPBot.java    |  3 +-
 .../TeleOp/States/SodiPizzaTeleOPState.java   | 62 ++++++-------------
 2 files changed, 22 insertions(+), 43 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java
index 79f4c90..7bb4f47 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java
@@ -30,7 +30,8 @@ public class MiniYTeleOPBot extends Robot {
         this.hardwareMap = CyberarmEngine.instance.hardwareMap;
         CyberarmEngine engine = CyberarmEngine.instance;
 
-        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+//        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+        configuration = new TimeCraftersConfiguration();
 
 
         imu = engine.hardwareMap.get(IMU.class, "imu");
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
index 1f65b1b..ae0e939 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
@@ -78,52 +78,30 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 
     @Override
     public void exec() {
-        
-        
-        
 
-        if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
-                Math.abs(engine.gamepad1.left_stick_x) < minInput &&
-                Math.abs(engine.gamepad1.right_stick_x) < minInput){
+        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
+        double x = engine.gamepad1.left_stick_x;
+        double rx = engine.gamepad1.right_stick_x;
+
+        double lbPower = (y - x + rx);
+        double rbPower = (y + x - rx);
+        double lfPower = (y + x + rx);
+        double rfPower = (y - x - rx);
+
+        robot.leftFront.setPower(lfPower * drivePower);
+        robot.leftBack.setPower(lbPower * drivePower);
+        robot.rightFront.setPower(rfPower * drivePower);
+        robot.rightBack.setPower(rbPower * drivePower);
+
+        if (engine.gamepad1.left_stick_x > 0.1) {
+            robot.leftBack.setPower(lbPower);
+            robot.rightBack.setPower(rbPower);
+            robot.leftFront.setPower(lfPower);
+            robot.rightFront.setPower(rfPower);
+
 
-            drivePower = 0;
-            robot.leftFront.setPower(drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(drivePower);
         }
 
-        if (Math.abs(yTransitPercent) > 0.01) {
-
-            percentDenom = 100;
-        } else {
-            percentDenom = 0;
-        }
-
-        if (Math.abs(xTransitPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
-        }
-
-        if (Math.abs(rotPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
-        }
-        yTransitPercent = engine.gamepad1.left_stick_y * 100;
-        xTransitPercent = engine.gamepad1.left_stick_x * 100;
-        rotPercent = engine.gamepad1.right_stick_x * -100;
-
-        lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
-        robot.leftFront.setPower(lfPower);
-
-        rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightFront.setPower(rfPower);
-
-        lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
-        robot.leftBack.setPower(lbPower);
-
-        rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightBack.setPower(rbPower);
 
         if (engine.gamepad2.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {