From 4df51dd52a31296b520d7f805f68a7b7bf7d646a Mon Sep 17 00:00:00 2001
From: "A. Teo Welton" <76081718+DragonDev07@users.noreply.github.com>
Date: Wed, 28 Feb 2024 16:53:39 -0700
Subject: [PATCH] New Drive Equations [TESTED, FUNCTIONING] (#108)

* New Drive Equations

* New Drive Equations fix
---
 .../teamcode/teleop/SingleDriverTeleOp.java   | 43 +++++++------------
 1 file changed, 16 insertions(+), 27 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java
index 8569ba2..530495b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java
@@ -144,33 +144,22 @@ public void loop() {
         robot.currentGamepad2.copy(gamepad2);
 
         // ------ Power & Controller Values ------ //
-        double leftFPower;
-        double rightFPower;
-        double leftBPower;
-        double rightBPower;
-        double turn = robot.currentGamepad1.left_stick_y * driveSpeed;
-        double drive = (robot.currentGamepad1.left_trigger - robot.currentGamepad1.right_trigger) * turnSpeed;
-        double strafe = -robot.currentGamepad1.left_stick_x * strafeSpeed;
+        double frontLeftPower;
+        double frontRightPower;
+        double backLeftPower;
+        double backRightPower;
+        double drive = -gamepad1.left_stick_y;
+        double strafe = gamepad1.left_stick_x;
+        double turn = (robot.currentGamepad1.left_trigger - robot.currentGamepad1.right_trigger) * turnSpeed;
         passoverPosition = robot.passoverArmLeft.getPosition();
         wristPosition = robot.wrist.getPosition();
 
         // ------ Calculate Drive Power ------ //
-        if (drive != 0 || turn != 0) {
-            leftFPower = Range.clip(drive + turn, -1.0, 1.0);
-            rightFPower = Range.clip(drive - turn, -1.0, 1.0);
-            leftBPower = Range.clip(drive + turn, -1.0, 1.0);
-            rightBPower = Range.clip(drive - turn, -1.0, 1.0);
-        } else if (strafe != 0) {
-            leftFPower = strafe;
-            rightFPower = strafe;
-            leftBPower = -strafe;
-            rightBPower = -strafe;
-        } else {
-            leftFPower = 0;
-            rightFPower = 0;
-            leftBPower = 0;
-            rightBPower = 0;
-        }
+        double denominator = Math.max(Math.abs(drive) + Math.abs(strafe) + Math.abs(turn), 1);
+        frontLeftPower = (turn - strafe - drive) / denominator;
+        backLeftPower = (turn + strafe - drive) / denominator;
+        frontRightPower = (turn - strafe + drive) / denominator;
+        backRightPower = (turn + strafe + drive) / denominator;
 
         // ------ (GAMEPAD 1) Claw Controls ------ //
         if (robot.currentGamepad1.x && !robot.previousGamepad1.x) {
@@ -267,10 +256,10 @@ public void loop() {
         }
 
         // ------ Run Motors ------ //
-        robot.leftFront.setPower(leftFPower);
-        robot.leftRear.setPower(leftBPower);
-        robot.rightFront.setPower(rightFPower);
-        robot.rightRear.setPower(rightBPower);
+        robot.leftFront.setPower(frontLeftPower);
+        robot.leftRear.setPower(backLeftPower);
+        robot.rightFront.setPower(frontRightPower);
+        robot.rightRear.setPower(backRightPower);
 
         // ------ Run Servos ------ //
         robot.passoverArmLeft.setPosition(passoverPosition);