teleop basically complete

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

@@ -102,7 +102,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
     public static double shoot0Time = 1.6;
     public static double intake1Time = 3.0;
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 25;
+    public static double pickup1Speed = 17;
     // ---- SECOND SHOT / PICKUP ----
     public static double shoot1Vel = 2300;
     public static double shoot1Hood = 0.93;
@@ -720,7 +720,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
                             new TranslationalVelConstraint(pickup1Speed));
 
             shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
             pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
@@ -729,7 +728,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
                             new TranslationalVelConstraint(pickup1Speed));
 
             shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
             pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
@@ -737,7 +735,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
                             new TranslationalVelConstraint(pickup1Speed));
             shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
 

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

@@ -111,7 +111,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
     public static double shoot0Time = 1.6;
     public static double intake1Time = 3.0;
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 25;
+    public static double pickup1Speed = 17;
     // ---- SECOND SHOT / PICKUP ----
     public static double shoot1Vel = 2300;
     public static double shoot1Hood = 0.93;
@@ -836,7 +836,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                             new TranslationalVelConstraint(pickup1Speed));
 
             shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
             pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
@@ -845,7 +844,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                             new TranslationalVelConstraint(pickup1Speed));
 
             shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
             pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
@@ -853,7 +851,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
                             new TranslationalVelConstraint(pickup1Speed));
             shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
-                    .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
             TELE.addData("Red?", redAlliance);

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

@@ -7,5 +7,5 @@ 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;
+    public static double colorFilterAlpha = 1;
 }

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

@@ -16,10 +16,11 @@ public class ServoPositions {
 
     public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
     public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
-    public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
+    public static double spinStartPos = 0.25;
+    public static double spinEndPos = 0.85;
 
     public static double shootAllAutoSpinStartPos = 0.2;
-    public static double shootAllSpindexerSpeedIncrease = 0.02;
+    public static double shootAllSpindexerSpeedIncrease = 0.04;
     public static double shootAllTime = 1.8;
 
     public static double transferServo_out = 0.15;

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

@@ -168,14 +168,6 @@ public class TeleopV3 extends LinearOpMode {
         PIDFController tController = new PIDFController(tp, ti, td, tf);
 
         tController.setTolerance(0.001);
-//
-//        if (redAlliance) {
-//            robot.limelight.pipelineSwitch(3);
-//        } else {
-//            robot.limelight.pipelineSwitch(2);
-//        }
-
-//        robot.limelight.start();
 
         Turret turret = new Turret(robot, TELE, robot.limelight);
         robot.light.setPosition(1);
@@ -264,57 +256,6 @@ public class TeleopV3 extends LinearOpMode {
 
             }
 
-//            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);
-////                        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);
-//
-//                    }
-//
-//                    spindexPos = spindexer_intakePos1;
-//
-//                    robot.intake.setPower(0);
-//
-//                    intakeTicker = 0;
-//                }
-//            }
-
             //COLOR:
 
             double s1D = robot.color1.getDistance(DistanceUnit.MM);
@@ -447,31 +388,6 @@ public class TeleopV3 extends LinearOpMode {
             //SHOOTER:
             flywheel.manageFlywheel(vel);
 
-            //TODO: test the camera teleop code
-
-//            if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
-//                double bearing;
-//
-//                LLResult result = robot.light.getLatestResult();
-//                if (result != null) {
-//                    if (result.isValid()) {
-//                        bearing = result.getTx() * bMult;
-//
-//                        double bearingCorrection = bearing / bDiv;
-//
-//                        error += bearingCorrection;
-//
-//                        camTicker++;
-//                        TELE.addData("tx", bearingCorrection);
-//                        TELE.addData("ty", result.getTy());
-//                    }
-//                }
-//
-//            } else {
-//                camTicker = 0;
-//                overrideTurr = false;
-//            }
-
             //HOOD:
 
             if (targetingHood) {
@@ -491,7 +407,6 @@ public class TeleopV3 extends LinearOpMode {
                 autoHoodOffset += 0.02;
 
             }
-//
 
             if (gamepad2.cross) {
                 manualOffset = 0;
@@ -527,77 +442,6 @@ public class TeleopV3 extends LinearOpMode {
 //                    yOffset = robotY;
 //                    headingOffset = robotHeading;
 //                }
-//            }
-
-            //if (gamepad1.left_bumper && !enableSpindexerManager) {
-
-            //    robot.transferServo.setPosition(transferServo_out);
-
-//                autoSpintake = false;
-//
-//                intakeTicker++;
-//
-//                if (intakeTicker % 10 < 1) {
-//
-//                    robot.spin1.setPower(-1);
-//                    robot.spin2.setPower(1);
-//
-//                } else if (intakeTicker % 10 < 5) {
-//                    robot.spin1.setPower(-0.5);
-//                    robot.spin2.setPower(0.5);
-//                } else if (intakeTicker % 10 < 6) {
-//                    robot.spin1.setPower(1);
-//                    robot.spin2.setPower(-1);
-//                } else {
-//                    robot.spin1.setPower(0.5);
-//                    robot.spin2.setPower(-0.5);
-//                }
-//
-//                intake = false;
-//                reject = false;
-//
-//                robot.intake.setPower(0.5);
-//
-//            }
-//
-//            if (gamepad1.leftBumperWasReleased() && !enableSpindexerManager) {
-//                shootStamp = getRuntime();
-//                shootAll = true;
-//
-//                shooterTicker = 0;
-//            }
-//
-//            if (shootAll && !enableSpindexerManager) {
-//
-//                TELE.addData("100% works", shootOrder);
-//
-//                intake = false;
-//                reject = false;
-//
-//                shooterTicker++;
-//
-//                spindexPos = spindexer_intakePos1;
-//
-//                if (getRuntime() - shootStamp < 3.5) {
-//
-//                    robot.transferServo.setPosition(transferServo_in);
-//
-//                    autoSpintake = false;
-//
-//                    robot.spin1.setPower(-spinPow);
-//                    robot.spin2.setPower(spinPow);
-//
-//                } else {
-//                    robot.transferServo.setPosition(transferServo_out);
-//                    spindexPos = spindexer_intakePos1;
-//
-//                    shootAll = false;
-//
-//                    autoSpintake = true;
-//
-//                    robot.transferServo.setPosition(transferServo_out);
-//                }
-//
 //            }
 
             if (enableSpindexerManager) {
@@ -672,12 +516,12 @@ public class TeleopV3 extends LinearOpMode {
 
 
                     if (shooterTicker == 0) {
-                        spindexer.prepareShootAll();
+                        spindexer.prepareShootAllContinous();
                         TELE.addLine("preparing to shoot");
-                    } else if (shooterTicker == 2) {
+//                    } else if (shooterTicker == 2) {
-                        //robot.transferServo.setPosition(transferServo_in);
+//                        //robot.transferServo.setPosition(transferServo_in);
-                        spindexer.shootAll();
+//                        spindexer.shootAll();
-                        TELE.addLine("starting to shoot");
+//                        TELE.addLine("starting to shoot");
                     } else if (!spindexer.shootAllComplete()) {
                         robot.transferServo.setPosition(transferServo_in);
                         TELE.addLine("shoot");
@@ -699,7 +543,6 @@ public class TeleopV3 extends LinearOpMode {
 
                     shootAll = false;
                     spindexer.resetSpindexer();
-                    spindexer.processIntake();
                 }
             }
 

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

@@ -5,6 +5,9 @@ import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.*;
+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;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
@@ -72,7 +75,9 @@ public class Spindexer {
         SHOOTMOVING,
         SHOOTWAIT,
         SHOOT_ALL_PREP,
-        SHOOT_ALL_READY
+        SHOOT_ALL_READY,
+        SHOOT_PREP_CONTINOUS,
+        SHOOT_CONTINOUS
     }
 
     int shootWaitCount = 0;
@@ -143,6 +148,9 @@ public class Spindexer {
         ballPositions[pos].isEmpty = true;
         ballPositions[pos].foundEmpty = 0;
         ballPositions[pos].ballColor = BallColor.UNKNOWN;
+        distanceRearCenter = 61;
+        distanceFrontDriver = 61;
+        distanceFrontPassenger = 61;
     }
 
     public void resetSpindexer () {
@@ -263,8 +271,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);
@@ -272,8 +280,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() {
@@ -309,6 +317,8 @@ public class Spindexer {
     public boolean isFull () {
         return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
     }
+
+    private double intakeTicker = 0;
     public boolean processIntake() {
 
         switch (currentIntakeState) {
@@ -359,15 +369,15 @@ public class Spindexer {
                     commandedIntakePosition = 0;
                     currentIntakeState = Spindexer.IntakeState.MOVING;
                 }
-                proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
+                //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
                 //if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
-                if (ballPositions[1].isEmpty) {
+                else if (ballPositions[1].isEmpty) {
                     // Position 2
                     commandedIntakePosition = 1;
                     currentIntakeState = Spindexer.IntakeState.MOVING;
                 }
-                proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
+                //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
-                if (ballPositions[2].isEmpty) {
+                else if (ballPositions[2].isEmpty) {
                     // Position 3
                     commandedIntakePosition = 2;
                     currentIntakeState = Spindexer.IntakeState.MOVING;
@@ -383,8 +393,13 @@ public class Spindexer {
             case MOVING:
                 // Stopping when we get to the new position
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
+                    if (intakeTicker > 1){
                         currentIntakeState = Spindexer.IntakeState.INTAKE;
                         stopSpindexer();
+                        intakeTicker = 0;
+                    } else {
+                        intakeTicker++;
+                    }
                     //detectBalls(false, false);
                 } else {
                     // Keep moving the spindexer
@@ -476,10 +491,33 @@ public class Spindexer {
                     //detectBalls(true, false);
                 }
                 // Keep moving the spindexer
-                spindexerOuttakeWiggle *= -1.01;
+                spindexerOuttakeWiggle *= -1.0;
                 moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
                 break;
 
+            case SHOOT_PREP_CONTINOUS:
+                if (servos.spinEqual(spinStartPos)){
+                    currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
+                } else {
+                    moveSpindexerToPos(spinStartPos);
+                }
+                break;
+
+            case SHOOT_CONTINOUS:
+                ballPositions[0].isEmpty = false;
+                ballPositions[1].isEmpty = false;
+                ballPositions[2].isEmpty = false;
+                if (servos.getSpinPos() > spinEndPos){
+                    currentIntakeState = IntakeState.FINDNEXT;
+                } else {
+                    double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
+                    if (spinPos > spinEndPos + 0.03){
+                        spinPos = spinEndPos + 0.03;
+                    }
+                    moveSpindexerToPos(spinPos);
+                }
+                break;
+
             default:
                 // Statements to execute if no case matches
         }
@@ -539,12 +577,21 @@ public class Spindexer {
     public void prepareShootAll(){
         currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
     }
+    public void prepareShootAllContinous(){
+        currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS;
+    }
     public void shootAll () {
         ballPositions[0].isEmpty = false;
         ballPositions[1].isEmpty = false;
         ballPositions[2].isEmpty = false;
         currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
     }
+    public void shootAllContinous(){
+        ballPositions[0].isEmpty = false;
+        ballPositions[1].isEmpty = false;
+        ballPositions[2].isEmpty = false;
+        currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
+    }
 
     public boolean shootAllComplete ()
     {
@@ -552,7 +599,9 @@ public class Spindexer {
                 (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
                 (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
                 (currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) &&
-                (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT));
+                (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT) &&
+                (currentIntakeState != Spindexer.IntakeState.SHOOT_PREP_CONTINOUS) &&
+                (currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS));
     }
 
     void shootAllToIntake () {