From 6286a2b71573b09c667e824e104819d67bdd0475 Mon Sep 17 00:00:00 2001
From: Sodi <ravenpaw5040@gmail.com>
Date: Sat, 29 Oct 2022 11:27:53 -0500
Subject: [PATCH] Slowing the button rotation on TeleOP, Sprint 3 TeleOP, take
 2.

---
 .../testing/states/PrototypeTeleOPState.java    | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
index 5b4af97..040b144 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
@@ -41,6 +41,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
         engine.telemetry.addData("AutoChain", AutoChain);
         engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
+        engine.telemetry.addData("Drive Power", drivePower);
 //        engine.telemetry.addData("Cycling Arm up / Down Case", CyclingArmUpAndDown);
 
     }
@@ -152,14 +153,14 @@ public class PrototypeTeleOPState extends CyberarmState {
             RobotRotation = robot.imu.getAngularOrientation().firstAngle;
             RotationTarget = -180;
             if (RobotRotation < 0 && RobotRotation > -177) {
-                drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (1 * (Math.abs(RobotRotation - RotationTarget)/180)) + 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation > 0) {
-                drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (-1 * (Math.abs(RobotRotation - RotationTarget)/180)) - 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -178,14 +179,14 @@ public class PrototypeTeleOPState extends CyberarmState {
             RobotRotation = robot.imu.getAngularOrientation().firstAngle;
             RotationTarget = 0;
             if (RobotRotation < -3) {
-                drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (-1 * (Math.abs(RobotRotation - RotationTarget)/180)) - 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation > 3) {
-                drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (1 * (Math.abs(RobotRotation - RotationTarget)/180)) + 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -203,14 +204,14 @@ public class PrototypeTeleOPState extends CyberarmState {
             RobotRotation = robot.imu.getAngularOrientation().firstAngle;
             RotationTarget = 135;
             if (RobotRotation > -45 && RobotRotation < 132) {//CCW
-                drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (-1 * (Math.abs(RobotRotation - RotationTarget)/180)) - 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation < -45 || RobotRotation > 138) {//CW
-                drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (1 * (Math.abs(RobotRotation - RotationTarget)/180)) + 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -228,14 +229,14 @@ public class PrototypeTeleOPState extends CyberarmState {
             RobotRotation = robot.imu.getAngularOrientation().firstAngle;
             RotationTarget = -135;
             if (RobotRotation < 45 && RobotRotation > -132) {//CCW
-                drivePower = 1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (1 * (Math.abs(RobotRotation - RotationTarget)/180)) + 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
                 robot.frontRightDrive.setPower(-drivePower);
             }
             if (RobotRotation > 45 || RobotRotation < -138) {//CW
-                drivePower = -1 * (Math.abs(RobotRotation - RotationTarget)/180) + 0.3;
+                drivePower = (-1 * (Math.abs(RobotRotation - RotationTarget)/180)) - 0.3;
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(-drivePower);
                 robot.frontLeftDrive.setPower(drivePower);