Lights work

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java

@@ -6,6 +6,11 @@ import com.acmerobotics.dashboard.config.Config;
 public class Color {
     public static boolean redAlliance = true;
     public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5;
+    public static double LightGreen = 0.5;
+    public static double LightPurple = 0.715;
+    public static double LightOrange = 0.33;
+    public static double LightRed = 0.28;
+    public static double LightBlue = 0.6;
 
     public static double colorFilterAlpha = 1;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/StateEnums.java

@@ -1,10 +1,19 @@
 package org.firstinspires.ftc.teamcode.constants;
 
-public class Types {
+public class StateEnums {
     public enum Motif {
         NONE,
         GPP,  // Green, Purple, Purple
         PGP,  // Purple, Green, Purple
         PPG   // Purple, Purple, Green
     }
+
+    public enum LightState {
+        BALL_COUNT,
+        BALL_COLOR,
+        GOAL_LOCK,
+        MANUAL,
+        DISABLED,
+        OFF
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -16,9 +16,12 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Drivetrain;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -47,6 +50,7 @@ public class TeleopV3 extends LinearOpMode {
     boolean fixedTurret = false;
     Robot robot;
     MultipleTelemetry TELE;
+    Light light;
     Servos servo;
     Flywheel flywheel;
     MecanumDrive drive;
@@ -63,7 +67,6 @@ public class TeleopV3 extends LinearOpMode {
     double headingOffset = 0.0;
     int ticker = 0;
 
-
     boolean autoSpintake = false;
     boolean enableSpindexerManager = true;
 
@@ -99,18 +102,26 @@ public class TeleopV3 extends LinearOpMode {
         tController.setTolerance(0.001);
 
         Turret turret = new Turret(robot, TELE, robot.limelight);
-        robot.light.setPosition(1);
+
+        light = Light.getInstance();
+        light.init(robot.light, spindexer, turret);
+
+        light.setState(StateEnums.LightState.MANUAL);
         while (opModeInInit()) {
             robot.limelight.start();
             if (redAlliance) {
                 robot.limelight.pipelineSwitch(4);
+                light.setManualLightColor(Color.LightRed);
             } else {
                 robot.limelight.pipelineSwitch(2);
+                light.setManualLightColor(Color.LightBlue);
+
             }
+
+            light.update();
         }
 
-        limelightUsed= true;
+        limelightUsed = true;
-
 
         waitForStart();
         if (isStopRequested()) return;
@@ -121,9 +132,6 @@ public class TeleopV3 extends LinearOpMode {
 
             //TELE.addData("Is limelight on?", robot.limelight.getStatus());
 
-            // LIGHT COLORS
-            spindexer.ballCounterLight();
-
             //DRIVETRAIN:
 
             drivetrain.drive(
@@ -134,9 +142,17 @@ public class TeleopV3 extends LinearOpMode {
             );
 
             if (gamepad1.right_bumper) {
+
                 shootAll = false;
                 robot.transferServo.setPosition(transferServo_out);
 
+                light.setState(StateEnums.LightState.BALL_COUNT);
+
+            } else if (gamepad2.triangle){
+                light.setState(StateEnums.LightState.BALL_COLOR);
+
+            }  else {
+                light.setState(StateEnums.LightState.GOAL_LOCK);
             }
 
             robot.transfer.setPower(1);
@@ -227,8 +243,6 @@ public class TeleopV3 extends LinearOpMode {
                 drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
             }
 
-
-
             if (enableSpindexerManager) {
                 //if (!shootAll) {
                 spindexer.processIntake();
@@ -332,7 +346,7 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData("robotX", robotX);
             TELE.addData("robotY", robotY);
             TELE.addData("robotInchesX", targeting.robotInchesX);
-            TELE.addData( "robotInchesY", targeting.robotInchesY);
+            TELE.addData("robotInchesY", targeting.robotInchesY);
             TELE.addData("Targeting Interpolate", turretInterpolate);
             TELE.addData("Targeting GridX", targeting.robotGridX);
             TELE.addData("Targeting GridY", targeting.robotGridY);
@@ -342,6 +356,8 @@ public class TeleopV3 extends LinearOpMode {
 
             TELE.update();
 
+            light.update();
+
             ticker++;
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java

@@ -0,0 +1,108 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.StateEnums.LightState;
+
+
+@Config
+public final class Light {
+
+    private static Light instance;
+    public static  double ballColorCycleTime = 1000; //in ms
+    public static double restingTime = 150; //in ms
+
+    private Servo lightServo;
+    private LightState state = LightState.DISABLED;
+
+    // References to other systems (NOT static)
+    private Spindexer spindexer;
+    private Turret turret;
+    private double manualLightColor = Color.Light0;
+
+    private double lightColor = Color.Light0;
+    private double previousLightColor = lightColor;
+
+    private Light() {
+    }
+
+    public static synchronized Light getInstance() {
+        if (instance == null) {
+            instance = new Light();
+        }
+        return instance;
+    }
+
+    // Call once in OpMode init()
+    public void init(
+            Servo servo,
+            Spindexer spin,
+            Turret turr
+    ) {
+        this.lightServo = servo;
+        this.spindexer = spin;
+        this.turret = turr;
+    }
+
+    public void setManualLightColor(double value) {
+        this.manualLightColor = value;
+    }
+
+    public void setState(LightState newState) {
+        state = newState;
+    }
+
+    public void update() {
+        if (lightServo == null) return;
+
+        switch (state) {
+
+            case BALL_COUNT:
+                lightColor = spindexer.ballCounterLight();
+                break;
+
+            case BALL_COLOR:
+
+                if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
+                    lightColor = spindexer.getRearCenterLight();
+                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) {
+                    lightColor = 0;
+                } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
+                    lightColor = spindexer.getDriverLight();
+
+                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
+                    lightColor = 0;
+                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
+                    lightColor = spindexer.getPassengerLight();
+
+                } else {
+                    lightColor = 0;
+                }
+                break;
+
+            case GOAL_LOCK:
+                lightColor = turret.getLightColor();
+                break;
+
+            case MANUAL:
+                lightColor = manualLightColor;
+                break;
+
+            case DISABLED:
+                break;
+
+            case OFF:
+                lightColor = 0;
+                break;
+        }
+
+        if (lightColor != previousLightColor) {
+            lightServo.setPosition(lightColor);
+        }
+
+        previousLightColor = lightColor;
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java

@@ -1,10 +1,13 @@
 package org.firstinspires.ftc.teamcode.utils;
 
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import static org.firstinspires.ftc.teamcode.constants.Color.Light0;
-import com.arcrobotics.ftclib.controller.PIDFController;
+import static org.firstinspires.ftc.teamcode.constants.Color.Light1;
-import com.qualcomm.robotcore.hardware.HardwareMap;
+import static org.firstinspires.ftc.teamcode.constants.Color.Light2;
-
+import static org.firstinspires.ftc.teamcode.constants.Color.Light3;
-import static org.firstinspires.ftc.teamcode.constants.Color.*;
+import static org.firstinspires.ftc.teamcode.constants.Color.LightGreen;
+import static org.firstinspires.ftc.teamcode.constants.Color.LightOrange;
+import static org.firstinspires.ftc.teamcode.constants.Color.LightPurple;
+import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
@@ -19,89 +22,51 @@ import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinP;
 
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.constants.Types;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 
+import java.util.Objects;
+
 public class Spindexer {
 
+    // Array of commandedIntakePositions with contents
+    // {RearCenter, FrontDriver, FrontPassenger}
+    static final int[][] RotatedBallPositions = {{0, 2, 1}, {1, 0, 2}, {2, 1, 0}};
+    public int commandedIntakePosition = 0;
+    public double distanceRearCenter = 0.0;
+    public double distanceFrontDriver = 0.0;
+    public double distanceFrontPassenger = 0.0;
+    public double spindexerWiggle = 0.01;
+    public double spindexerOuttakeWiggle = 0.01;
+    public double spindexerPosOffset = 0.00;
+    public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
+    public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
+    public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
+    public int unknownColorDetect = 0;
+    public int counter = 0;
     Robot robot;
     Servos servos;
     Flywheel flywheel;
     MecanumDrive drive;
     double lastKnownSpinPos = 0.0;
     MultipleTelemetry TELE;
-
     PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
-
     double spinCurrentPos = 0.0;
-
-    public int commandedIntakePosition = 0;
-
-    public double distanceRearCenter = 0.0;
-    public double distanceFrontDriver = 0.0;
-    public double distanceFrontPassenger = 0.0;
-
-    public double spindexerWiggle = 0.01;
-
-    public double spindexerOuttakeWiggle = 0.01;
-    private double prevPos = 0.0;
-    public double spindexerPosOffset = 0.00;
-    public Types.Motif desiredMotif = Types.Motif.NONE;
-    // For Use
-    enum RotatedBallPositionNames {
-        REARCENTER,
-        FRONTDRIVER,
-        FRONTPASSENGER
-    }
-    // Array of commandedIntakePositions with contents
-    // {RearCenter, FrontDriver, FrontPassenger}
-    static final int[][] RotatedBallPositions = {{0,2,1}, {1,0,2}, {2,1,0}};
-    class spindexerBallRoatation {
-        int rearCenter = 0; // aka commanded Position
-        int frontDriver = 0;
-        int frontPassenger = 0;
-    }
-
-    enum IntakeState {
-        UNKNOWN_START,
-        UNKNOWN_MOVE,
-        UNKNOWN_DETECT,
-        INTAKE,
-        FINDNEXT,
-        MOVING,
-        FULL,
-        SHOOTNEXT,
-        SHOOTMOVING,
-        SHOOTWAIT,
-        SHOOT_ALL_PREP,
-        SHOOT_ALL_READY,
-        SHOOT_PREP_CONTINOUS,
-        SHOOT_CONTINOUS
-    }
-
     int shootWaitCount = 0;
-
-    public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
-    public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
-    public int unknownColorDetect = 0;
-    public enum BallColor {
-        UNKNOWN,
-        GREEN,
-        PURPLE
-    }
-
-    class BallPosition {
-        boolean isEmpty = true;
-        int foundEmpty = 0;
-        BallColor ballColor = BallColor.UNKNOWN;
-    }
-
     BallPosition[] ballPositions = new BallPosition[3];
-
+    double[] outakePositions =
-    public boolean init () {
+            {spindexer_outtakeBall1 + spindexerPosOffset,
-        return true;
+                    spindexer_outtakeBall2 + spindexerPosOffset,
-    }
+                    spindexer_outtakeBall3 + spindexerPosOffset};
+    double[] intakePositions =
+            {spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
+    private double prevPos = 0.0;
+    private double intakeTicker = 0;
 
     public Spindexer(HardwareMap hardwareMap) {
         robot = new Robot(hardwareMap);
@@ -115,36 +80,13 @@ public class Spindexer {
         ballPositions[1] = new BallPosition();
         ballPositions[2] = new BallPosition();
 
-
     }
 
+    public boolean init() {
+        return true;
+    }
 
-    double[] outakePositions =
+    public void resetBallPosition(int pos) {
-            {spindexer_outtakeBall1+spindexerPosOffset,
-                    spindexer_outtakeBall2+spindexerPosOffset,
-                    spindexer_outtakeBall3+spindexerPosOffset};
-
-    double[] intakePositions =
-            {spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
-
-    public int counter = 0;
-
-//    private double getTimeSeconds ()
-//    {
-//        return (double) System.currentTimeMillis()/1000.0;
-//    }
-
-//    public double getPos() {
-//        robot.spin1Pos.getVoltage();
-//        robot.spin1Pos.getMaxVoltage();
-//        return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
-//    }
-
-//    public void manageSpindexer() {
-//
-//    }
-
-    public void resetBallPosition (int pos) {
         ballPositions[pos].isEmpty = true;
         ballPositions[pos].foundEmpty = 0;
         ballPositions[pos].ballColor = BallColor.UNKNOWN;
@@ -153,7 +95,7 @@ public class Spindexer {
         distanceFrontPassenger = 61;
     }
 
-    public void resetSpindexer () {
+    public void resetSpindexer() {
         for (int i = 0; i < 3; i++) {
             resetBallPosition(i);
         }
@@ -170,11 +112,11 @@ public class Spindexer {
 
         // Read Distances
         double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
-        distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter);
+        distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1 - colorFilterAlpha) * distanceRearCenter);
         double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
-        distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver);
+        distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1 - colorFilterAlpha) * distanceFrontDriver);
         double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
-        distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
+        distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1 - colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
         if (distanceRearCenter < 60) {
@@ -196,7 +138,7 @@ public class Spindexer {
                 } else {
                     ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE;  // purple
                 }
-           }
+            }
         }
         // Position 2
         // Find which ball position this is in the spindexer
@@ -267,10 +209,11 @@ public class Spindexer {
 
         return newPos1Detection;
     }
+
     // Has code to unjam spindexer
     private void moveSpindexerToPos(double pos) {
         robot.spin1.setPosition(pos);
-        robot.spin2.setPosition(1-pos);
+        robot.spin2.setPosition(1 - pos);
 //        double currentPos = servos.getSpinPos();
 //        if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
 //            if (currentPos > pos){
@@ -284,41 +227,89 @@ public class Spindexer {
 //        prevPos = currentPos;
     }
 
+//    private double getTimeSeconds ()
+//    {
+//        return (double) System.currentTimeMillis()/1000.0;
+//    }
+
+//    public double getPos() {
+//        robot.spin1Pos.getVoltage();
+//        robot.spin1Pos.getMaxVoltage();
+//        return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
+//    }
+
+//    public void manageSpindexer() {
+//
+//    }
+
     public void stopSpindexer() {
 
     }
 
-    public void ballCounterLight(){
+    public double getRearCenterLight() {
-        int counter = 0;
+        BallColor color = GetRearCenterColor();
-        if (!ballPositions[0].isEmpty){
+        if (Objects.equals(color, BallColor.GREEN)) {
-            counter++;
+            return LightGreen;
-        }
+        } else if (Objects.equals(color, BallColor.PURPLE)) {
-        if (!ballPositions[1].isEmpty){
+            return LightPurple;
-            counter++;
-        }
-        if (!ballPositions[2].isEmpty){
-            counter++;
-        }
-        if (counter == 3){
-            robot.light.setPosition(Light3);
-        } else if (counter == 2){
-            robot.light.setPosition(Light2);
-        } else if (counter == 1){
-            robot.light.setPosition(Light1);
         } else {
-            robot.light.setPosition(Light0);
+            return LightOrange;
         }
     }
 
-    public boolean slotIsEmpty(int slot){
+
+    public double getDriverLight() {
+        BallColor color = GetFrontDriverColor();
+        if (Objects.equals(color, BallColor.GREEN)) {
+            return LightGreen;
+        } else if (Objects.equals(color, BallColor.PURPLE)) {
+            return LightPurple;
+        } else {
+            return LightOrange;
+        }
+    }
+
+    public double getPassengerLight() {
+        BallColor color = GetFrontPassengerColor();
+        if (Objects.equals(color, BallColor.GREEN)) {
+            return LightGreen;
+        } else if (Objects.equals(color, BallColor.PURPLE)) {
+            return LightPurple;
+        } else {
+            return LightOrange;
+        }
+    }
+    public double ballCounterLight() {
+        int counter = 0;
+        if (!ballPositions[0].isEmpty) {
+            counter++;
+        }
+        if (!ballPositions[1].isEmpty) {
+            counter++;
+        }
+        if (!ballPositions[2].isEmpty) {
+            counter++;
+        }
+
+        if (counter == 3) {
+            return Light3;
+        } else if (counter == 2) {
+            return Light2;
+        } else if (counter == 1) {
+            return Light1;
+        } else {
+            return Light0;
+        }
+    }
+
+    public boolean slotIsEmpty(int slot) {
         return !ballPositions[slot].isEmpty;
     }
 
-    public boolean isFull () {
+    public boolean isFull() {
         return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
     }
 
-    private double intakeTicker = 0;
     public boolean processIntake() {
 
         switch (currentIntakeState) {
@@ -340,7 +331,7 @@ public class Spindexer {
                 }
                 break;
             case UNKNOWN_DETECT:
-                if (unknownColorDetect >5) {
+                if (unknownColorDetect > 5) {
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
                 } else {
                     //detectBalls(true, true);
@@ -355,7 +346,7 @@ public class Spindexer {
                 } else {
                     // Maintain Position
                     spindexerWiggle *= -1.0;
-                    moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
+                    moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
                 }
                 break;
             case FINDNEXT:
@@ -393,7 +384,7 @@ public class Spindexer {
             case MOVING:
                 // Stopping when we get to the new position
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
-                    if (intakeTicker > 1){
+                    if (intakeTicker > 1) {
                         currentIntakeState = Spindexer.IntakeState.INTAKE;
                         stopSpindexer();
                         intakeTicker = 0;
@@ -416,7 +407,7 @@ public class Spindexer {
                 }
                 // Maintain Position
                 spindexerWiggle *= -1.0;
-                moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
+                moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
                 break;
 
             case SHOOT_ALL_PREP:
@@ -475,7 +466,7 @@ public class Spindexer {
                 double shootWaitMax = 4;
                 // Stopping when we get to the new position
                 if (prevIntakeState != currentIntakeState) {
-                    if (commandedIntakePosition==2) {
+                    if (commandedIntakePosition == 2) {
                         shootWaitMax = 5;
                     }
                     shootWaitCount = 0;
@@ -492,11 +483,11 @@ public class Spindexer {
                 }
                 // Keep moving the spindexer
                 spindexerOuttakeWiggle *= -1.0;
-                moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
+                moveSpindexerToPos(outakePositions[commandedIntakePosition] + spindexerOuttakeWiggle);
                 break;
 
             case SHOOT_PREP_CONTINOUS:
-                if (servos.spinEqual(spinStartPos)){
+                if (servos.spinEqual(spinStartPos)) {
                     currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
                 } else {
                     moveSpindexerToPos(spinStartPos);
@@ -507,11 +498,11 @@ public class Spindexer {
                 ballPositions[0].isEmpty = false;
                 ballPositions[1].isEmpty = false;
                 ballPositions[2].isEmpty = false;
-                if (servos.getSpinPos() > spinEndPos){
+                if (servos.getSpinPos() > spinEndPos) {
                     currentIntakeState = IntakeState.FINDNEXT;
                 } else {
                     double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
-                    if (spinPos > spinEndPos + 0.03){
+                    if (spinPos > spinEndPos + 0.03) {
                         spinPos = spinEndPos + 0.03;
                     }
                     moveSpindexerToPos(spinPos);
@@ -529,12 +520,12 @@ public class Spindexer {
         return false;
     }
 
-    public void setDesiredMotif (Types.Motif newMotif) {
+    public void setDesiredMotif(StateEnums.Motif newMotif) {
         desiredMotif = newMotif;
     }
 
     // Returns the best fit for the motiff
-    public int bestFitMotif () {
+    public int bestFitMotif() {
         switch (desiredMotif) {
             case GPP:
                 if (ballPositions[0].ballColor == BallColor.GREEN) {
@@ -565,36 +556,38 @@ public class Spindexer {
                 //break;
             case NONE:
                 return 0;
-                //break;
+            //break;
         }
         return 0;
     }
 
-    void prepareToShootMotif () {
+    void prepareToShootMotif() {
         commandedIntakePosition = bestFitMotif();
     }
 
-    public void prepareShootAll(){
+    public void prepareShootAll() {
         currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
     }
-    public void prepareShootAllContinous(){
+
+    public void prepareShootAllContinous() {
         currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS;
     }
-    public void shootAll () {
+
+    public void shootAll() {
         ballPositions[0].isEmpty = false;
         ballPositions[1].isEmpty = false;
         ballPositions[2].isEmpty = false;
         currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
     }
-    public void shootAllContinous(){
+
+    public void shootAllContinous() {
         ballPositions[0].isEmpty = false;
         ballPositions[1].isEmpty = false;
         ballPositions[2].isEmpty = false;
         currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
     }
 
-    public boolean shootAllComplete ()
+    public boolean shootAllComplete() {
-    {
         return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
                 (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
                 (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
@@ -604,23 +597,64 @@ public class Spindexer {
                 (currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS));
     }
 
-    void shootAllToIntake () {
+    void shootAllToIntake() {
         currentIntakeState = Spindexer.IntakeState.FINDNEXT;
     }
 
-    public void update()
+    public void update() {
-    {
     }
 
-    public BallColor GetFrontDriverColor () {
+    public BallColor GetFrontDriverColor() {
         return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
     }
 
-    public BallColor GetFrontPassengerColor () {
+    public BallColor GetFrontPassengerColor() {
         return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
     }
 
-    public BallColor GetRearCenterColor () {
+    public BallColor GetRearCenterColor() {
         return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
     }
+
+    // For Use
+    enum RotatedBallPositionNames {
+        REARCENTER,
+        FRONTDRIVER,
+        FRONTPASSENGER
+    }
+
+    enum IntakeState {
+        UNKNOWN_START,
+        UNKNOWN_MOVE,
+        UNKNOWN_DETECT,
+        INTAKE,
+        FINDNEXT,
+        MOVING,
+        FULL,
+        SHOOTNEXT,
+        SHOOTMOVING,
+        SHOOTWAIT,
+        SHOOT_ALL_PREP,
+        SHOOT_ALL_READY,
+        SHOOT_PREP_CONTINOUS,
+        SHOOT_CONTINOUS
+    }
+
+    public enum BallColor {
+        UNKNOWN,
+        GREEN,
+        PURPLE
+    }
+
+    class spindexerBallRoatation {
+        int rearCenter = 0; // aka commanded Position
+        int frontDriver = 0;
+        int frontPassenger = 0;
+    }
+
+    class BallPosition {
+        boolean isEmpty = true;
+        int foundEmpty = 0;
+        BallColor ballColor = BallColor.UNKNOWN;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

@@ -9,10 +9,10 @@ import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.teamcode.constants.Color;
 
 import java.util.List;
 
@@ -51,6 +51,7 @@ public class Turret {
     private int obeliskID = 0;
     private double offset = 0.0;
     private double currentTrackOffset = 0.0;
+    private double lightColor = Color.LightRed;
     private int currentTrackCount = 0;
     private double permanentOffset = 0.0;
     private PIDController bearingPID;
@@ -62,6 +63,10 @@ public class Turret {
         bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
     }
 
+    public double getLightColor() {
+        return lightColor;
+    }
+
     public void zeroTurretEncoder() {
         robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -153,11 +158,17 @@ public class Turret {
         // LL has 54.5 degree total Horizontal FOV; very edges are not useful.
         final double HORIZONTAL_FOV_RANGE = 26.0;  // Total usable horizontal degrees from center +/-
         final double DRIVE_POWER_REDUCTION = 2.0;
+        final double COLOR_OK_TOLERANCE = 2.5;
 
         if (abs(targetTx) < TARGET_POSITION_TOLERANCE) {
             bearingAligned = true;
-        } else {
+            lightColor = Color.LightBlue;
+        } else if (abs(targetTx) < COLOR_OK_TOLERANCE) {
             bearingAligned = false;
+            lightColor = Color.LightPurple;
+        }  else {
+            bearingAligned = false;
+            lightColor = Color.LightOrange;
         }
 
         // Only with valid data and if too far off target
@@ -246,6 +257,7 @@ public class Turret {
 //            if (currentTrackCount > 20) {
 //                offset = currentTrackOffset;
 //            }
+            lightColor = Color.LightRed;
             currentTrackOffset = 0.0;
             currentTrackCount = 0;
         }