From 558be5987b9da8ff672448416193e49c1a8d9302 Mon Sep 17 00:00:00 2001
From: Yash Karandikar <yash@karx.xyz>
Date: Wed, 9 Nov 2022 18:17:18 -0600
Subject: [PATCH] Auto stuff

---
 .../enhancedautos/AutoBlueColorSensor.java    | 16 +++++++---
 .../enhancedautos/AutoRedColorSensor.java     | 32 +++++++++++++++----
 2 files changed, 37 insertions(+), 11 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java
index 29db6e6..d204d25 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java
@@ -15,7 +15,9 @@ public class AutoBlueColorSensor extends LinearOpMode {
         waitForStart();
 
         movementAPI.move(0, 0.7);
-        api.pause(0.25);
+        api.pause(0.4);
+        movementAPI.move(-90, 0.7);
+        api.pause(0.15);
         movementAPI.stop();
 
         api.pause(5);
@@ -25,9 +27,7 @@ public class AutoBlueColorSensor extends LinearOpMode {
 
         if (r < 100 && g < 100 && b < 100) {
             movementAPI.move(-180, 0.7);
-            api.pause(0.25);
-            movementAPI.move(-180, 0.7);
-            api.pause(0.5);
+            api.pause(0.75);
             movementAPI.move(90, 0.7);
             api.pause(0.7);
             movementAPI.stop();
@@ -43,16 +43,24 @@ public class AutoBlueColorSensor extends LinearOpMode {
 
         if (largest == g) {
             // move to position 1
+            movementAPI.move(-180, 0.7);
+            api.pause(0.75);
             movementAPI.move(90, 0.7);
             api.pause(0.25);
+            movementAPI.move(180, 0.7);
+            api.pause(0.75);
         } else if (largest == r) {
             // move to position 2
             movementAPI.move(0, 0.7);
             api.pause(0.25);
         } else if (largest == b) {
             // move to position 3
+            movementAPI.move(-180, 0.7);
+            api.pause(0.75);
             movementAPI.move(-90, 0.7);
             api.pause(0.25);
+            movementAPI.move(180, 0.7);
+            api.pause(0.75);
         }
 
         movementAPI.stop();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java
index 0dad3a0..8a45799 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java
@@ -15,7 +15,9 @@ public class AutoRedColorSensor extends LinearOpMode {
         waitForStart();
 
         movementAPI.move(0, 0.7);
-        api.pause(0.5);
+        api.pause(0.4);
+        movementAPI.move(90, 0.7);
+        api.pause(0.15);
         movementAPI.stop();
 
         api.pause(5);
@@ -23,26 +25,42 @@ public class AutoRedColorSensor extends LinearOpMode {
         int g = sensor.green();
         int b = sensor.blue();
 
+        if (r < 100 && g < 100 && b < 100) {
+            movementAPI.move(-180, 0.7);
+            api.pause(0.75);
+            movementAPI.move(-90, 0.7);
+            api.pause(0.7);
+            movementAPI.stop();
+
+            terminateOpModeNow();
+        }
+
         int largest = api.getLargest(r, g, b);
 
+        api.print("r", String.valueOf(r));
+        api.print("g", String.valueOf(g));
+        api.print("b", String.valueOf(b));
+
         if (largest == g) {
             // move to position 1
+            movementAPI.move(-180, 0.7);
+            api.pause(0.75);
             movementAPI.move(-90, 0.7);
             api.pause(0.25);
+            movementAPI.move(180, 0.7);
+            api.pause(0.75);
         } else if (largest == r) {
             // move to position 2
             movementAPI.move(0, 0.7);
             api.pause(0.25);
         } else if (largest == b) {
             // move to position 3
+            movementAPI.move(-180, 0.7);
+            api.pause(0.75);
             movementAPI.move(90, 0.7);
             api.pause(0.25);
-        } else {
-            // Color sensor broke, move to backup position
-            movementAPI.move(-180, 0.7);
-            api.pause(0.5);
-            movementAPI.move(-90, 0.7);
-            api.pause(0.7);
+            movementAPI.move(180, 0.7);
+            api.pause(0.75);
         }
 
         movementAPI.stop();