working teleop in progress

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

@@ -1,6 +1,11 @@
 package org.firstinspires.ftc.teamcode.constants;
 
+import com.acmerobotics.dashboard.config.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 colorFilterAlpha = 0.15;
 }

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
@@ -178,6 +179,15 @@ public class TeleopV3 extends LinearOpMode {
 
         Turret turret = new Turret(robot, TELE, robot.limelight);
         robot.light.setPosition(1);
+        while (opModeInInit()){
+            robot.limelight.start();
+            if (redAlliance) {
+                robot.limelight.pipelineSwitch(4);
+            } else {
+                robot.limelight.pipelineSwitch(2);
+            }
+        }
+
         waitForStart();
         if (isStopRequested()) return;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -18,6 +20,9 @@ public class ColorTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        double color1Distance = 0;
+        double color2Distance = 0;
+        double color3Distance = 0;
 
         waitForStart();
         if (isStopRequested()) return;
@@ -26,28 +31,34 @@ public class ColorTest extends LinearOpMode {
             double green1 = robot.color1.getNormalizedColors().green;
             double blue1 = robot.color1.getNormalizedColors().blue;
             double red1 = robot.color1.getNormalizedColors().red;
+            double dist1 = robot.color1.getDistance(DistanceUnit.MM);
+            color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance);
 
             TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
             TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
-            TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
+            TELE.addData("Color1 distance (mm)", color1Distance);
 
 // ----- COLOR 2 -----
             double green2 = robot.color2.getNormalizedColors().green;
             double blue2 = robot.color2.getNormalizedColors().blue;
             double red2 = robot.color2.getNormalizedColors().red;
+            double dist2 = robot.color2.getDistance(DistanceUnit.MM);
+            color2Distance = (colorFilterAlpha * dist2) + ((1-colorFilterAlpha) * color2Distance);
 
             TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
             TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
-            TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
+            TELE.addData("Color2 distance (mm)", color2Distance);
 
 // ----- COLOR 3 -----
             double green3 = robot.color3.getNormalizedColors().green;
             double blue3 = robot.color3.getNormalizedColors().blue;
             double red3 = robot.color3.getNormalizedColors().red;
+            double dist3 = robot.color3.getDistance(DistanceUnit.MM);
+            color3Distance = (colorFilterAlpha * dist3) + ((1-colorFilterAlpha) * color3Distance);
 
             TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
             TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
-            TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
+            TELE.addData("Color3 distance (mm)", color3Distance);
 
             TELE.update();
         }

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

@@ -161,12 +161,15 @@ public class Spindexer {
         int spindexerBallPos = 0;
 
         // Read Distances
-        distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM);
+        double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
-        distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
+        distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter);
-        distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
+        double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
+        distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver);
+        double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
+        distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 50) {
+        if (distanceRearCenter < 60) {
 
             // Mark Ball Found
             newPos1Detection = true;
@@ -218,7 +221,7 @@ public class Spindexer {
 
         // Position 3
         spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
-        if (distanceFrontPassenger < 25) {
+        if (distanceFrontPassenger < 29) {
 
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
@@ -260,8 +263,8 @@ public class Spindexer {
     private void moveSpindexerToPos(double pos) {
         robot.spin1.setPosition(pos);
         robot.spin2.setPosition(1-pos);
-//        double currentPos = servos.getSpinPos();
+        double currentPos = servos.getSpinPos();
-//        if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
+        if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
 //            if (currentPos > pos){
 //                robot.spin1.setPosition(servos.getSpinPos() + 0.05);
 //                robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
@@ -269,8 +272,8 @@ public class Spindexer {
 //                robot.spin1.setPosition(servos.getSpinPos() - 0.05);
 //                robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
 //            }
-//        }
+        }
-//        prevPos = currentPos;
+        prevPos = currentPos;
     }
 
     public void stopSpindexer() {
@@ -341,7 +344,8 @@ public class Spindexer {
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
                 } else {
                     // Maintain Position
-                    moveSpindexerToPos(intakePositions[commandedIntakePosition]);
+                    spindexerWiggle *= -1.0;
+                    moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
                 }
                 break;
             case FINDNEXT:

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

@@ -61,12 +61,6 @@ public class Turret {
         this.TELE = tele;
         this.robot = rob;
         this.webcam = cam;
-        webcam.start();
-        if (redAlliance) {
-            webcam.pipelineSwitch(4);
-        } else {
-            webcam.pipelineSwitch(2);
-        }
         bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
     }