From 9aab8fadf10587e4bd2904ae86927232a6cb7199 Mon Sep 17 00:00:00 2001
From: robotics2 <robotics2@scds.org>
Date: Thu, 5 Oct 2023 17:12:37 -0700
Subject: [PATCH] color read up and knows sides

---
 .../ftc/teamcode/Autonomoustest.java          | 51 ++++++++++++++++---
 1 file changed, 45 insertions(+), 6 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java
index b3e66c4..2e7a128 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java
@@ -71,6 +71,7 @@ public class Autonomoustest extends LinearOpMode {
     private DcMotor         backrightDrive  = null;
     private DcMotor         backleftDrive  = null;
     private ColorSensor  colorRight = null;
+    private ColorSensor colorLeft = null;
 
 
 
@@ -100,6 +101,7 @@ public class Autonomoustest extends LinearOpMode {
         backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
         backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
         colorRight = hardwareMap.get(ColorSensor.class, "color right");
+        colorLeft = hardwareMap.get(ColorSensor.class, "color left");
         // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
         // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
         // Note: The settings here assume direct drive on left and right wheels.  Gear Reduction or 90 Deg drives may require direction flips
@@ -119,11 +121,11 @@ public class Autonomoustest extends LinearOpMode {
         backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         // Send telemetry message to indicate successful Encoder reset
-        telemetry.addData("Starting at",  "%7d :%7d",
+       /* telemetry.addData("Starting at",  "%7d :%7d",
                           leftDrive.getCurrentPosition(),
                           rightDrive.getCurrentPosition(),
                 backleftDrive.getCurrentPosition(),
-        backrightDrive.getCurrentPosition());
+        backrightDrive.getCurrentPosition());*/
 
         telemetry.update();
 
@@ -133,9 +135,31 @@ public class Autonomoustest extends LinearOpMode {
         // Step through each leg of the path,
         // Note: Reverse movement is obtained by setting a negative distance (not speed)
         //driveForward(2);
-        //driveForward(80);
-        readColor();
-        //turnRight(90);
+        loop();
+        while(opModeIsActive())
+        {
+            int blueleft = readColorLeft();
+            int blueright = readColorRight();
+           if (blueleft > 50)
+           {
+               //telemetry.addData("color sensor","left");
+               if(blueleft > blueright)
+                   telemetry.addData("color sensor","left");
+
+
+           }
+           if (blueright > 50)
+           {
+               //telemetry.addData("color sensor", "right");
+               if(blueleft < blueright)
+                   telemetry.addData("color sensor","right");
+           }
+           else
+               telemetry.addData("position","center");
+           telemetry.update();
+            sleep(250);
+
+        }
 
 
 
@@ -188,12 +212,27 @@ public class Autonomoustest extends LinearOpMode {
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
         encoderDrive(DRIVE_SPEED,  turning_distance,  turning_distance, LONG_TIMEOUT);
     }
-    public void readColor()
+    public int readColorRight()
     {
         telemetry.addData("Clear", colorRight.alpha());
         telemetry.addData("Red  ", colorRight.red());
         telemetry.addData("Green", colorRight.green());
         telemetry.addData("Blue ", colorRight.blue());
+        int bluenumber = colorRight.blue();
+        return bluenumber;
+    }
+    public int readColorLeft()
+    {
+        telemetry.addData("Clear Left", colorLeft.alpha());
+        telemetry.addData("Red left ", colorLeft.red());
+        telemetry.addData("Green left", colorLeft.green());
+        telemetry.addData("Blue left", colorLeft.blue());
+
+        int bluenumber = colorLeft.blue();
+        return bluenumber;
+
+
+
     }
 
     /*