stash

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

@@ -7,14 +7,6 @@ import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.*;
-import static org.firstinspires.ftc.teamcode.constants.Color.Light0;
-import static org.firstinspires.ftc.teamcode.constants.Color.Light1;
-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.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;
@@ -29,30 +21,36 @@ 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.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 
-import java.util.Objects;
-
 public class Spindexer {
 
-    // Array of commandedIntakePositions with contents
+    Robot robot;
-    // {RearCenter, FrontDriver, FrontPassenger}
+    Servos servos;
-    static final int[][] RotatedBallPositions = {{0, 2, 1}, {1, 0, 2}, {2, 1, 0}};
+    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 static int shootWaitMax = 4;
-    public Types.Motif desiredMotif = Types.Motif.NONE;
+    public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
     // For Use
     enum RotatedBallPositionNames {
         REARCENTER,
@@ -87,29 +85,26 @@ public class Spindexer {
 
     int shootWaitCount = 0;
 
-    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;
+    public enum BallColor {
-    Robot robot;
+        UNKNOWN,
-    Servos servos;
+        GREEN,
-    Flywheel flywheel;
+        PURPLE
-    MecanumDrive drive;
+    }
-    double lastKnownSpinPos = 0.0;
+
-    MultipleTelemetry TELE;
+    class BallPosition {
-    PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
+        boolean isEmpty = true;
-    double spinCurrentPos = 0.0;
+        int foundEmpty = 0;
-    int shootWaitCount = 0;
+        BallColor ballColor = BallColor.UNKNOWN;
+    }
+
     BallPosition[] ballPositions = new BallPosition[3];
-    double[] outakePositions =
+
-            {spindexer_outtakeBall1 + spindexerPosOffset,
+    public boolean init () {
-                    spindexer_outtakeBall2 + spindexerPosOffset,
+        return true;
-                    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);
@@ -123,13 +118,36 @@ public class Spindexer {
         ballPositions[1] = new BallPosition();
         ballPositions[2] = new BallPosition();
 
+
     }
 
-    public boolean init() {
-        return true;
-    }
 
-    public void resetBallPosition(int pos) {
+    double[] outakePositions =
+            {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;
@@ -138,7 +156,7 @@ public class Spindexer {
         distanceFrontPassenger = 61;
     }
 
-    public void resetSpindexer() {
+    public void resetSpindexer () {
         for (int i = 0; i < 3; i++) {
             resetBallPosition(i);
         }
@@ -155,11 +173,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) {
@@ -246,7 +264,6 @@ public class Spindexer {
 
         return newPos1Detection;
     }
-
     // Has code to unjam spindexer
     private void moveSpindexerToPos(double pos) {
         servos.setSpinPos(pos);
@@ -263,69 +280,20 @@ public class Spindexer {
 //        prevPos = pos;
     }
 
-//    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() {
 
     }
 
     private double prevLight = 0.0;
     public void ballCounterLight(){
-    public double getRearCenterLight() {
-        BallColor color = GetRearCenterColor();
-        if (Objects.equals(color, BallColor.GREEN)) {
-            return LightGreen;
-        } else if (Objects.equals(color, BallColor.PURPLE)) {
-            return LightPurple;
-        } else {
-            return LightOrange;
-        }
-    }
-
-
-    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) {
+        if (!ballPositions[0].isEmpty){
             counter++;
         }
-        if (!ballPositions[1].isEmpty) {
+        if (!ballPositions[1].isEmpty){
             counter++;
         }
-        if (!ballPositions[2].isEmpty) {
+        if (!ballPositions[2].isEmpty){
             counter++;
         }
 
@@ -336,16 +304,8 @@ public class Spindexer {
             light = Light2;
         } else if (counter == 1){
             light = Light1;
-
-        if (counter == 3) {
-            return Light3;
-        } else if (counter == 2) {
-            return Light2;
-        } else if (counter == 1) {
-            return Light1;
         } else {
             light = Light0;
-            return Light0;
         }
         if (light != prevLight){
             robot.light.setPosition(light);
@@ -353,14 +313,15 @@ public class Spindexer {
         prevLight = light;
     }
 
-    public boolean slotIsEmpty(int slot) {
+    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) {
@@ -382,7 +343,7 @@ public class Spindexer {
                 }
                 break;
             case UNKNOWN_DETECT:
-                if (unknownColorDetect > 5) {
+                if (unknownColorDetect >5) {
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
                 } else {
                     //detectBalls(true, true);
@@ -435,7 +396,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;
@@ -516,7 +477,7 @@ public class Spindexer {
             case SHOOTWAIT:
                 // Stopping when we get to the new position
                 if (prevIntakeState != currentIntakeState) {
-                    if (commandedIntakePosition == 2) {
+                    if (commandedIntakePosition==2) {
                         shootWaitMax = 5;
                     }
                     shootWaitCount = 0;
@@ -537,7 +498,7 @@ public class Spindexer {
                 break;
 
             case SHOOT_PREP_CONTINOUS:
-                if (servos.spinEqual(spinStartPos)) {
+                if (servos.spinEqual(spinStartPos)){
                     currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
                 } else {
                     servos.setSpinPos(spinStartPos);
@@ -548,7 +509,7 @@ 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 = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
@@ -570,12 +531,12 @@ public class Spindexer {
         return false;
     }
 
-    public void setDesiredMotif(StateEnums.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) {
@@ -611,33 +572,31 @@ public class Spindexer {
         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) &&
@@ -647,66 +606,25 @@ 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;
-    }
     private double prevPow = 0.501;
     public void setIntakePower(double pow){
         if (prevPow != 0.501 && prevPow != pow){