Add functions to get the ball color to spindexer. Attempt to make shoot all in teleop work better.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java

@@ -309,7 +309,6 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                     return false;
 
                 }
-
             }
         };
     }

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

@@ -11,12 +11,15 @@ public class ServoPositions {
 
     public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
 
-    public static double spindexer_outtakeBall2 = 0.51; //0.46; //0.6;
+    public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
-    public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4;
+    public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
     public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
 
+    public static double shootAllAutoSpinStartPos = 0.2;
+    public static double shootAllSpindexerSpeedIncrease = 0.02;
+    public static double shootAllTime = 1.8;
 
     public static double transferServo_out = 0.15;
 

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

@@ -31,6 +31,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -250,56 +251,56 @@ public class TeleopV3 extends LinearOpMode {
 
             }
 
-            if (autoSpintake) {
+//            if (autoSpintake) {
-
-                if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
-
-
-                    robot.spin1.setPosition(spindexPos);
-                    robot.spin2.setPosition(1-spindexPos);
-
-                }
-
-                if (gamepad1.right_bumper) {
-
-                    shootAll = false;
-
-                    intakeTicker++;
-
-//                    if (intakeTicker % 20 < 2) {
 //
-//                        robot.spin1.setPower(-1);
+//                if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
-//                        robot.spin2.setPower(1);
+//
+//
+//                    robot.spin1.setPosition(spindexPos);
+//                    robot.spin2.setPosition(1-spindexPos);
+//
+//                }
+//
+//                if (gamepad1.right_bumper) {
+//
+//                    shootAll = false;
+//
+//                    intakeTicker++;
+//
+////                    if (intakeTicker % 20 < 2) {
+////
+////                        robot.spin1.setPower(-1);
+////                        robot.spin2.setPower(1);
+////
+////                    } else if (intakeTicker % 20 < 10) {
+////                        robot.spin1.setPower(-0.5);
+////                        robot.spin2.setPower(0.5);
+////                    } else if (intakeTicker % 20 < 12) {
+////                        robot.spin1.setPower(1);
+////                        robot.spin2.setPower(-1);
+////                    } else {
+////                        robot.spin1.setPower(0.5);
+////                        robot.spin2.setPower(-0.5);
+////                    }
+//
+//                    robot.intake.setPower(1);
+//                    intakeStamp = getRuntime();
+//                    TELE.addData("Reverse?", reverse);
+//                    TELE.update();
+//                } else {
+//                    if (!servo.spinEqual(spindexPos)) {
+//                        robot.spin1.setPosition(spindexPos);
+//                        robot.spin2.setPosition(1-spindexPos);
 //
-//                    } else if (intakeTicker % 20 < 10) {
-//                        robot.spin1.setPower(-0.5);
-//                        robot.spin2.setPower(0.5);
-//                    } else if (intakeTicker % 20 < 12) {
-//                        robot.spin1.setPower(1);
-//                        robot.spin2.setPower(-1);
-//                    } else {
-//                        robot.spin1.setPower(0.5);
-//                        robot.spin2.setPower(-0.5);
 //                    }
-
+//
-                    robot.intake.setPower(1);
+//                    spindexPos = spindexer_intakePos1;
-                    intakeStamp = getRuntime();
+//
-                    TELE.addData("Reverse?", reverse);
+//                    robot.intake.setPower(0);
-                    TELE.update();
+//
-                } else {
+//                    intakeTicker = 0;
-                    if (!servo.spinEqual(spindexPos)) {
+//                }
-                        robot.spin1.setPosition(spindexPos);
+//            }
-                        robot.spin2.setPosition(1-spindexPos);
-
-                    }
-
-                    spindexPos = spindexer_intakePos1;
-
-                    robot.intake.setPower(0);
-
-                    intakeTicker = 0;
-                }
-            }
 
             //COLOR:
 
@@ -587,7 +588,9 @@ public class TeleopV3 extends LinearOpMode {
 //            }
 
             if (enableSpindexerManager) {
-                spindexer.processIntake();
+                //if (!shootAll) {
+                    spindexer.processIntake();
+                //}
 
                 // RIGHT_BUMPER
                 if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
@@ -598,10 +601,7 @@ public class TeleopV3 extends LinearOpMode {
                 }
 
                 // LEFT_BUMPER
-                if (!shootAll &&
+                if (!shootAll && gamepad1.leftBumperWasReleased()) {
-                        (gamepad1.leftBumperWasReleased() ||
-                                gamepad1.leftBumperWasPressed() ||
-                                gamepad1.left_bumper)) {
                     shootStamp = getRuntime();
                     shootAll = true;
 
@@ -632,11 +632,37 @@ public class TeleopV3 extends LinearOpMode {
 //                            robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
 //                            TELE.addLine("shooting");
 //                        }
+
+//                    //robot.intake.setPower(-0.3);
+//                    if (getRuntime() - shootStamp < 3.0) {
+//
+//                        if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) {
+//                            robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos);
+//                            robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos);
+//                        } else {
+//                            shooterTicker++;
+//                            //robot.intake.setPower(0.0);
+//                            robot.transferServo.setPosition(transferServo_in);
+//                            double prevSpinPos = robot.spin1.getPosition();
+//                            robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease);
+//                            robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos);
+//                        }
+//
+//                    } else {
+//                        robot.transferServo.setPosition(transferServo_out);
+//                        //spindexPos = spindexer_intakePos1;
+//                        shootAll = false;
+//                        spindexer.resetSpindexer();
+//                        spindexer.processIntake();
+//
+//                    }
+
+
                     if (shooterTicker == 0) {
                         spindexer.prepareShootAll();
                         TELE.addLine("preparing to shoot");
                     } else if (shooterTicker == 2) {
-                        robot.transferServo.setPosition(transferServo_in);
+                        //robot.transferServo.setPosition(transferServo_in);
                         spindexer.shootAll();
                         TELE.addLine("starting to shoot");
                     } else if (!spindexer.shootAllComplete()) {
@@ -865,10 +891,10 @@ public class TeleopV3 extends LinearOpMode {
 //            TELE.addData("robotInchesX", targeting.robotInchesX);
 //            TELE.addData( "robotInchesY", targeting.robotInchesY);
 //            TELE.addData("Targeting Interpolate", turretInterpolate);
-//            TELE.addData("Targeting GridX", targeting.robotGridX);
+            TELE.addData("Targeting GridX", targeting.robotGridX);
-//            TELE.addData("Targeting GridY", targeting.robotGridY);
+            TELE.addData("Targeting GridY", targeting.robotGridY);
-//            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
+            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
-//            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
+            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
 //            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
 
             TELE.update();

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

@@ -77,7 +77,7 @@ public class Spindexer {
     public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
     public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
     public int unknownColorDetect = 0;
-    enum BallColor {
+    public enum BallColor {
         UNKNOWN,
         GREEN,
         PURPLE
@@ -397,6 +397,7 @@ public class Spindexer {
             case SHOOT_ALL_PREP:
                 // We get here with function call to prepareToShootMotif
                 // Stopping when we get to the new position
+                commandedIntakePosition = 0;
                 if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
                     // Keep moving the spindexer
                     moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
@@ -416,17 +417,17 @@ public class Spindexer {
 
             case SHOOTNEXT:
                 // Find Next Open Position and start movement
-                if (!ballPositions[1].isEmpty) {
+                if (!ballPositions[0].isEmpty) {
                     // Position 1
+                    commandedIntakePosition = 0;
+                    currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
+                } else if (!ballPositions[1].isEmpty) {
+                    // Position 2
                     commandedIntakePosition = 1;
                     currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
                 } else if (!ballPositions[2].isEmpty) {
-                    // Position 2
-                    commandedIntakePosition = 2;
-                    currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
-                } else if (!ballPositions[0].isEmpty) {
                     // Position 3
-                    commandedIntakePosition = 0;
+                    commandedIntakePosition = 2;
                     currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
                 } else {
                     // Empty return to intake state
@@ -446,14 +447,18 @@ public class Spindexer {
                 break;
 
             case SHOOTWAIT:
+                double shootWaitMax = 3;
                 // Stopping when we get to the new position
                 if (prevIntakeState != currentIntakeState) {
+                    if (commandedIntakePosition==2) {
+                        shootWaitMax = 5;
+                    }
                     shootWaitCount = 0;
                 } else {
                     shootWaitCount++;
                 }
                 // wait 10 cycles
-                if (shootWaitCount > 2) {
+                if (shootWaitCount > shootWaitMax) {
                     currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
                     ballPositions[commandedIntakePosition].isEmpty = true;
                     shootWaitCount = 0;
@@ -461,7 +466,7 @@ public class Spindexer {
                     //detectBalls(true, false);
                 }
                 // Keep moving the spindexer
-                moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01));
+                //moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.02));
                 break;
 
             default:
@@ -524,7 +529,10 @@ public class Spindexer {
         currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
     }
     public void shootAll () {
-        currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
+        ballPositions[0].isEmpty = false;
+        ballPositions[1].isEmpty = false;
+        ballPositions[2].isEmpty = false;
+        currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
     }
 
     public boolean shootAllComplete ()
@@ -543,4 +551,16 @@ public class Spindexer {
     public void update()
     {
     }
+
+    public BallColor GetFrontDriverColor () {
+        return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
+    }
+
+    public BallColor GetFrontPassengerColor () {
+        return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
+    }
+
+    public BallColor GetRearCenterColor () {
+        return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
+    }
 }