back gate auto is like 90% done and changed some things to reduce warnings

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

@@ -2,6 +2,12 @@ package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
@@ -33,24 +39,12 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
-    public static double spindexerSpeedIncrease = 0.008;
+    public static double spindexerSpeedIncrease = 0.014;
-
-    // These values are ADDED to turrDefault
-    public static double redObeliskTurrPos1 = 0.12;
-    public static double redObeliskTurrPos2 = 0.13;
-    public static double redObeliskTurrPos3 = 0.14;
-    public static double blueObeliskTurrPos1 = -0.12;
-    public static double blueObeliskTurrPos2 = -0.13;
-    public static double blueObeliskTurrPos3 = -0.14;
-    public static double redTurretShootPos = 0.1;
-    public static double blueTurretShootPos = -0.14;
 
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
 
-    double turretShootPos = 0.0;
-
     public static double shootAllTime = 2;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
@@ -97,8 +91,6 @@ public class Auto_LT_Close extends LinearOpMode {
     double xPrep, yPrep, hPrep;
     double xLeave, yLeave, hLeave;
 
-    private double shoot1Tangent;
-
     int ballCycles = 3;
     int prevMotif = 0;
 
@@ -218,8 +210,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
-                turretShootPos = turrDefault + redTurretShootPos;
-
             } else {
                 robot.light.setPosition(0.6);
 
@@ -261,8 +251,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
-                turretShootPos = turrDefault + blueTurretShootPos;
-
             }
 
             shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
@@ -355,15 +343,6 @@ public class Auto_LT_Close extends LinearOpMode {
     void shoot(){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                shootAllTime,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501,
-                                false
-                        ),
                         autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
                 )
 
@@ -380,12 +359,8 @@ public class Auto_LT_Close extends LinearOpMode {
                                 flywheel0Time,
                                 x1,
                                 y1,
-                                posXTolerance,
+                                h1
-                                posYTolerance,
-                                h1,
-                                false
                         )
-
                 )
         );
     }
@@ -394,16 +369,12 @@ public class Auto_LT_Close extends LinearOpMode {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
-                        autoActions.manageShooterAuto(
+                        autoActions.intake(
                                 intake1Time,
                                 x2b,
                                 y2b,
-                                posXTolerance,
+                                h2b
-                                posYTolerance,
-                                h2b,
-                                true
                         ),
-                        autoActions.intake(intake1Time),
                         autoActions.detectObelisk(
                                 intake1Time,
                                 x2b,
@@ -436,17 +407,15 @@ public class Auto_LT_Close extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
+                        shoot1.build(),
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
                                 shoot1Time,
+                                motif,
                                 posX,
                                 posY,
-                                posXTolerance,
+                                posH
-                                posYTolerance,
+                        )
-                                posH,
-                                false
-                        ),
-                        shoot1.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
                 )
         );
     }
@@ -455,16 +424,12 @@ public class Auto_LT_Close extends LinearOpMode {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup2.build(),
-                        autoActions.manageShooterAuto(
+                        autoActions.intake(
                                 intake2Time,
                                 x3b,
                                 y3b,
-                                posXTolerance,
+                                h3b
-                                posYTolerance,
-                                h3b,
-                                true
                         ),
-                        autoActions.intake(intake2Time),
                         autoActions.detectObelisk(
                                 intake2Time,
                                 x3b,
@@ -497,17 +462,14 @@ public class Auto_LT_Close extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
+                        shoot2.build(),
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
                                 shoot2Time,
+                                motif,
                                 posX,
                                 posY,
-                                posXTolerance,
+                                posH)
-                                posYTolerance,
-                                posH,
-                                false
-                        ),
-                        shoot2.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
                 )
         );
     }
@@ -516,16 +478,12 @@ public class Auto_LT_Close extends LinearOpMode {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup3.build(),
-                        autoActions.manageShooterAuto(
+                        autoActions.intake(
                                 intake3Time,
                                 x4b,
                                 y4b,
-                                posXTolerance,
+                                h4b
-                                posYTolerance,
-                                h4b,
-                                true
                         ),
-                        autoActions.intake(intake3Time),
                         autoActions.detectObelisk(
                                 intake3Time,
                                 x4b,
@@ -545,17 +503,15 @@ public class Auto_LT_Close extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
+                        shoot3.build(),
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
                                 shoot3Time,
+                                motif,
                                 xLeave,
                                 yLeave,
-                                posXTolerance,
+                                hLeave
-                                posYTolerance,
+                        )
-                                hLeave,
-                                false
-                        ),
-                        shoot3.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
                 )
         );
     }

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

@@ -3,6 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueTurretShootPos;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
@@ -34,8 +42,6 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
     public static double shoot0Vel = 3300, shoot0Hood = 0.48;
-    public static double redTurretShootPos = 0.05;
-    public static double blueTurretShootPos = -0.05;
     double xLeave, yLeave, hLeave;
     public int motif = 0;
     double turretShootPos = 0.0;
@@ -60,26 +66,20 @@ public class Auto_LT_Far extends LinearOpMode {
     public static int pickupStackSpeed = 12;
     public static int pickupGateSpeed = 25;
     int prevMotif = 0;
-    public static double spindexerSpeedIncrease = 0.008;
+    public static double spindexerSpeedIncrease = 0.014;
     public static double shootAllTime = 2;
     // ---- POSITION TOLERANCES ----
     public static double posXTolerance = 5.0;
     public static double posYTolerance = 5.0;
     public static double shootStackTime = 2;
-    public static double shootGateTime = 2;
+    public static double shootGateTime = 2.5;
     public static double colorSenseTime = 1;
-    public static double intakeStackTime = 3;
+    public static double intakeStackTime = 2.5;
-    public static double intakeGateTime = 2.5;
+    public static double intakeGateTime = 2;
-    public static double redObeliskTurrPos1 = 0.12;
-    public static double redObeliskTurrPos2 = 0.13;
-    public static double redObeliskTurrPos3 = 0.14;
-    public static double blueObeliskTurrPos1 = -0.12;
-    public static double blueObeliskTurrPos2 = -0.13;
-    public static double blueObeliskTurrPos3 = -0.14;
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
-    public static double endAutoTime = 25;
+    public static double endAutoTime = 26;
 
     // initialize path variables here
     TrajectoryActionBuilder leave3Ball = null;
@@ -276,7 +276,14 @@ public class Auto_LT_Far extends LinearOpMode {
             }
 
             while (gatePickup && getRuntime() - stamp < endAutoTime){
-                cycleGatePickup();
+                cycleGatePickupBalls();
+                if (getRuntime() - stamp > endAutoTime){
+                    break;
+                }
+                cycleGatePrepareShoot();
+                if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
+                    break;
+                }
                 shoot();
             }
 
@@ -307,15 +314,6 @@ public class Auto_LT_Far extends LinearOpMode {
     void shoot(){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                shootAllTime,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501,
-                                false
-                        ),
                         autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
                 )
 
@@ -329,10 +327,7 @@ public class Auto_LT_Far extends LinearOpMode {
                                 flywheel0Time,
                                 0.501,
                                 0.501,
-                                0.501,
+                                0.501
-                                0.501,
-                                0.501,
-                                false
                         )
 
                 )
@@ -353,16 +348,12 @@ public class Auto_LT_Far extends LinearOpMode {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup3.build(),
-                        autoActions.manageShooterAuto(
+                        autoActions.intake(
                                 intakeStackTime,
                                 xStackPickupB,
                                 yStackPickupB,
-                                posXTolerance,
+                                hStackPickupB
-                                posYTolerance,
-                                hStackPickupB,
-                                true
                         ),
-                        autoActions.intake(intakeStackTime),
                         autoActions.detectObelisk(
                                 intakeStackTime,
                                 xStackPickupB,
@@ -382,35 +373,28 @@ public class Auto_LT_Far extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
+                        shoot3.build(),
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
                                 shootStackTime,
+                                motif,
                                 xShoot,
                                 yShoot,
-                                posXTolerance,
+                                hShoot)
-                                posYTolerance,
-                                hShoot,
-                                false
-                        ),
-                        shoot3.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
                 )
         );
     }
 
-    void cycleGatePickup(){
+    void cycleGatePickupBalls(){
         Actions.runBlocking(
                 new ParallelAction(
                         pickupGate.build(),
-                        autoActions.manageShooterAuto(
+                        autoActions.intake(
-                                intakeGateTime,
+                                intakeStackTime,
                                 pickupGateX,
                                 pickupGateY,
-                                posXTolerance,
+                                pickupGateH
-                                posYTolerance,
-                                pickupGateH,
-                                true
                         ),
-                        autoActions.intake(intakeStackTime),
                         autoActions.detectObelisk(
                                 intakeGateTime,
                                 pickupGateX,
@@ -426,20 +410,20 @@ public class Auto_LT_Far extends LinearOpMode {
 
         if (motif == 0) motif = prevMotif;
         prevMotif = motif;
+    }
 
+    void cycleGatePrepareShoot(){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageShooterAuto(
+                        shootGate.build(),
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
                                 shootGateTime,
+                                motif,
                                 xShoot,
                                 yShoot,
-                                posXTolerance,
+                                hShoot
-                                posYTolerance,
+                        )
-                                hShoot,
-                                false
-                        ),
-                        shootGate.build(),
-                        autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
                 )
         );
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
-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_outtakeBall1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -10,7 +9,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
-import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 
 import androidx.annotation.NonNull;
 
@@ -49,9 +47,8 @@ public class AutoActions{
     private int passengerSlotGreen = 0;
     private int rearSlotGreen = 0;
     private int mostGreenSlot = 0;
-    private double firstSpindexShootPos = spinStartPos;
+    private double firstSpindexShootPos = spindexer_outtakeBall1;
     private boolean shootForward = true;
-    public static double firstShootTime = 0.3;
     public int motif = 0;
     double spinEndPos = ServoPositions.spinEndPos;
 
@@ -68,7 +65,14 @@ public class AutoActions{
         this.light = lig;
     }
 
-    public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
+    public Action prepareShootAll(
+            double colorSenseTime,
+            double time,
+            int motif_id,
+            double posX,
+            double posY,
+            double posH
+    ) {
         return new Action() {
             double stamp = 0.0;
             int ticker = 0;
@@ -107,10 +111,12 @@ public class AutoActions{
                 spinEndPos = spindexer_outtakeBall2 + 0.1;
             }
 
+            Action manageShooter = null;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
+                    manageShooter = manageShooterAuto(time, posX, posY, posH);
                 }
                 ticker++;
                 servos.setTransferPos(transferServo_out);
@@ -120,6 +126,8 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
+                manageShooter.run(telemetryPacket);
+
                 if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
 
                     spindexerWiggle *= -1.0;
@@ -199,22 +207,16 @@ public class AutoActions{
         };
     }
 
-    private boolean doneShooting = false;
     public Action shootAllAuto(double shootTime, double spindexSpeed) {
         return new Action() {
             int ticker = 1;
 
             double stamp = 0.0;
 
-            double velo = 0.0;
-
             int shooterTicker = 0;
-
+            Action manageShooter = null;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
-                velo = flywheel.getVelo();
-
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
@@ -226,9 +228,13 @@ public class AutoActions{
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
+                    manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501);
+
                 }
                 ticker++;
 
+                manageShooter.run(telemetryPacket);
+
                 double prevSpinPos = servos.getSpinCmdPos();
 
                 boolean end;
@@ -238,18 +244,18 @@ public class AutoActions{
                     end = prevSpinPos < spinEndPos;
                 }
 
-                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
+                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
 
-                    if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
+                    if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
                         servos.setTransferPos(transferServo_out);
                         servos.setSpinPos(firstSpindexShootPos);
                     } else {
                         servos.setTransferPos(transferServo_in);
                         shooterTicker++;
-
+                        Spindexer.whileShooting = true;
-                        if (shootForward) {
+                        if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
                             servos.setSpinPos(prevSpinPos + spindexSpeed);
-                        } else {
+                        } else if (shooterTicker > Spindexer.waitFirstBallTicks){
                             servos.setSpinPos(prevSpinPos - spindexSpeed);
                         }
 
@@ -259,10 +265,9 @@ public class AutoActions{
 
                 } else {
                     servos.setTransferPos(transferServo_out);
-
+                    Spindexer.whileShooting = false;
                     spindexer.resetSpindexer();
                     spindexer.processIntake();
-                    doneShooting = true;
 
                     return false;
 
@@ -271,15 +276,21 @@ public class AutoActions{
         };
     }
 
-    public Action intake(double intakeTime) {
+    public Action intake(
+            double time,
+            double posX,
+            double posY,
+            double posH
+    ) {
         return new Action() {
             double stamp = 0.0;
             int ticker = 0;
-
+            Action manageShooter = null;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
+                    manageShooter = manageShooterAuto(time, posX, posY, posH);
                 }
                 ticker++;
 
@@ -292,7 +303,9 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
+                manageShooter.run(telemetryPacket);
+
+                return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull();
             }
         };
     }
@@ -360,22 +373,15 @@ public class AutoActions{
             double time,
             double posX,
             double posY,
-            double posXTolerance,
+            double posH
-            double posYTolerance,
-            double posH,
-            boolean whileIntaking
     ) {
 
         return new Action() {
 
             double stamp = 0.0;
             int ticker = 0;
-            int shootingTicker = 0;
-            double shootingStamp = 0;
 
             final boolean timeFallback = (time != 0.501);
-            final boolean posXFallback = (posX != 0.501);
-            final boolean posYFallback = (posY != 0.501);
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -406,7 +412,7 @@ public class AutoActions{
                     deltaPose = new Pose2d(robotX, robotY, robotHeading);
                 }
 
-                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+//                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
                 targetingSettings = targeting.calculateSettings
                         (robotX, robotY, robotHeading, 0.0, false);
@@ -418,31 +424,18 @@ public class AutoActions{
                 servos.setHoodPos(targetingSettings.hoodAngle);
 
                 double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
+                flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
-                boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
+                boolean shouldFinish = timeDone || flywheel.getSteady();
-                boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
-                boolean shouldFinish;
-                if (whileIntaking) {
-                    shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
-                } else {
-                    shouldFinish = timeDone || (xDone && yDone) || doneShooting;
-                }
 
                 teleStart = currentPose;
 
                 TELE.addData("Steady?", flywheel.getSteady());
                 TELE.update();
 
-                if (shouldFinish) {
+                return !shouldFinish;
-                    doneShooting = false;
-                    return false;
-                } else {
-                    return true;
-                }
-
             }
         };
     }

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

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.constants;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.Pose2d;
 
 @Config
 public class Back_Poses {
@@ -14,10 +13,10 @@ public class Back_Poses {
     public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
     public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
 
-    public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1;
+    public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
     public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
 
-    public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140;
+    public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
     public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140;
 
     public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;

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

@@ -39,8 +39,8 @@ public class Front_Poses {
     public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
     public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
 
-    public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
+    public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
-    public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
+    public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

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

@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.07; //0.13;
+    public static double spindexer_intakePos1 = 0.06; //0.13;
 
-    public static double spindexer_intakePos2 = 0.27; //0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.25; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.46; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
 
     public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
     public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24;
 
     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 = 0.22;
+    public static double spinStartPos = 0.3;
     public static double spinEndPos = 0.85;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;
@@ -39,7 +39,7 @@ public class ServoPositions {
     public static double blueObeliskTurrPos1 = -0.12;
     public static double blueObeliskTurrPos2 = -0.13;
     public static double blueObeliskTurrPos3 = -0.14;
-    public static double redTurretShootPos = 0.1;
+    public static double redTurretShootPos = 0.05;
-    public static double blueTurretShootPos = -0.14;
+    public static double blueTurretShootPos = -0.05;
 
 }

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

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.teleop;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
@@ -46,9 +45,9 @@ public class TeleopV3 extends LinearOpMode {
     public double vel = 3000;
     public boolean autoVel = true;
     public boolean targetingHood = true;
-    public boolean autoHood = true;
+//    public boolean autoHood = true;
     public double shootStamp = 0.0;
-    boolean fixedTurret = false;
+//    boolean fixedTurret = false;
     Robot robot;
     MultipleTelemetry TELE;
     Light light;
@@ -66,13 +65,13 @@ public class TeleopV3 extends LinearOpMode {
     boolean reject = false;
     double xOffset = 0.0;
     double yOffset = 0.0;
-    double headingOffset = 0.0;
+//    double headingOffset = 0.0;
     int ticker = 0;
 
-    boolean autoSpintake = false;
+//    boolean autoSpintake = false;
     boolean enableSpindexerManager = true;
 
-    boolean overrideTurr = false;
+//    boolean overrideTurr = false;
 
     int intakeTicker = 0;
     private boolean shootAll = false;
@@ -178,7 +177,7 @@ public class TeleopV3 extends LinearOpMode {
             double dy = robotY - goalY;  // delta y from robot to goal
             Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
 
-            double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+//            double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
             targetingSettings = targeting.calculateSettings
                     (robotX, robotY, robotHeading, 0.0, turretInterpolate);
@@ -210,7 +209,7 @@ public class TeleopV3 extends LinearOpMode {
 
             //SHOOTER:
             double voltage = robot.voltage.getVoltage();
-            flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
+            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
             flywheel.manageFlywheel(vel);
 
             //HOOD:
@@ -262,7 +261,6 @@ public class TeleopV3 extends LinearOpMode {
                     spindexer.setIntakePower(1);
                 } else if (gamepad1.cross) {
                     spindexer.setIntakePower(-1);
-
                 } else {
                     spindexer.setIntakePower(0);
                 }
@@ -288,11 +286,7 @@ public class TeleopV3 extends LinearOpMode {
 //                        //servo.setTransferPos(transferServo_in);
 //                        spindexer.shootAll();
 //                        TELE.addLine("starting to shoot");
-                    } else if (!spindexer.shootAllComplete()) {
+                    } else if (spindexer.shootAllComplete()) {
-                        servo.setTransferPos(transferServo_in);
-                        //TELE.addLine("shoot");
-                    } else {
-                        servo.setTransferPos(transferServo_out);
                         //spindexPos = spindexer_intakePos1;
                         shootAll = false;
                         spindexer.resetSpindexer();

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

@@ -37,7 +37,7 @@ public class ShooterTest extends LinearOpMode {
     public static double P = 255.0;
     public static double I = 0.0;
     public static double D = 0.0;
-    public static double F = 90;
+    public static double F = 75;
     public static double transferPower = 1.0;
     public static double hoodPos = 0.501;
     public static double turretPos = 0.501;

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

@@ -12,15 +12,13 @@ public class Flywheel {
     public double velo1 = 0.0;
     public double velo2 = 0.0;
     double targetVelocity = 0.0;
-    double previousTargetVelocity = 0.0;
-    double powPID = 0.0;
     boolean steady = false;
     public Flywheel (HardwareMap hardwareMap) {
         robot = new Robot(hardwareMap);
         shooterPIDF1 = new PIDFCoefficients
-                (robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
+                (Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
         shooterPIDF2 = new PIDFCoefficients
-                (robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
+                (Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
     }
 
     public double getVelo () {
@@ -52,6 +50,7 @@ public class Flywheel {
         if (Math.abs(prevF - f) > voltagePIDFDifference){
             robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
             robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
+            prevF = f;
         }
     }
 
@@ -61,26 +60,23 @@ public class Flywheel {
     // Convert from Ticks per Second to RPM
     private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
 
-    public double manageFlywheel(double commandedVelocity) {
+    public void manageFlywheel(double commandedVelocity) {
 
         if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
             targetVelocity = commandedVelocity;
             // Add code here to set PIDF based on desired RPM
             //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
             //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
-            robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-            robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
-
-            // Record Current Velocity
-            velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
-            velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
-            velo = Math.max(velo1, velo2);
 
         }
+        robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
+        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+        velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
+        velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
+        velo = Math.max(velo1, velo2);
         // really should be a running average of the last 5
-        steady = (Math.abs(commandedVelocity - velo) < 200.0);
+        steady = (Math.abs(commandedVelocity - velo) < 50);
 
-        return powPID;
     }
 
     public void update()

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

@@ -7,10 +7,10 @@ public class MeasuringLoopTimes {
     private double minLoopTime = 999999999999.0;
 
     private double maxLoopTime = 0.0;
-    private double mainLoopTime = 0.0;
+    double mainLoopTime = 0.0;
 
     private double MeasurementStart = 0.0;
-    private double currentTime = 0.0;
+    double currentTime = 0.0;
 
     private double avgLoopTime = 0.0;
     private int avgLoopTimeTicker = 0;

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

@@ -1,11 +1,9 @@
 package org.firstinspires.ftc.teamcode.utils;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.arcrobotics.ftclib.hardware.ServoEx;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
-import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -31,11 +29,11 @@ public class Robot {
     public DcMotorEx intake;
     public DcMotorEx transfer;
     public PIDFCoefficients shooterPIDF;
-    public double shooterPIDF_P = 255.0;
+    public static double shooterPIDF_P = 255;
-    public double shooterPIDF_I = 0.0;
+    public static double shooterPIDF_I = 0.0;
-    public double shooterPIDF_D = 0.0;
+    public static double shooterPIDF_D = 0.0;
-    public double shooterPIDF_F = 90;
+    public static double shooterPIDF_F = 75;
-    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
+//    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;

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

@@ -14,8 +14,6 @@ public class Servos {
     public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0;
     public static double spin_scalar = 1.112;
     public static double spin_restPos = 0.155;
-    public static double turret_scalar = 1.009;
-    public static double turret_restPos = 0.0;
     Robot robot;
     PIDFController spinPID;
     PIDFController turretPID;
@@ -51,14 +49,13 @@ public class Servos {
         return (Math.abs(pos1 - pos2) < 0.005);
     }
 
-    public double setTransferPos(double pos) {
+    public void setTransferPos(double pos) {
         if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
             robot.transferServo.setPosition(pos);
             firstTransferPos = false;
         }
 
         prevTransferPos = pos;
-        return pos;
     }
 
     public double setSpinPos(double pos) {
@@ -72,29 +69,16 @@ public class Servos {
         return pos;
     }
 
-    public double setHoodPos(double pos){
+    public void setHoodPos(double pos){
         if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
             robot.hood.setPosition(pos + hoodOffset);
             firstHoodPos = false;
         }
 
         prevHoodPos = pos;
-        return pos;
     }
 
     public boolean spinEqual(double pos) {
         return Math.abs(pos - this.getSpinPos()) < 0.03;
     }
-
-    public double getTurrPos() {
-        return 1.0;
-    }
-
-    public double setTurrPos(double pos) {
-        return 1.0;
-    }
-
-    public boolean turretEqual(double pos) {
-        return true;
-    }
 }

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

@@ -16,6 +16,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
@@ -52,6 +54,9 @@ public class Spindexer {
     private double prevPos = 0.0;
     public double spindexerPosOffset = 0.00;
     public static int shootWaitMax = 4;
+    public static boolean whileShooting = false;
+    public static int waitFirstBallTicks = 4;
+    private int shootTicks = 0;
     public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
     // For Use
     enum RotatedBallPositionNames {
@@ -530,18 +535,26 @@ public class Spindexer {
                 break;
 
             case SHOOT_PREP_CONTINOUS:
-                if (servos.spinEqual(spinStartPos)){
+                if (shootTicks > waitFirstBallTicks){
                     currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
+                    shootTicks++;
+                } else if (servos.spinEqual(spinStartPos)){
+                    shootTicks++;
+                    servos.setTransferPos(transferServo_in);
                 } else {
                     servos.setSpinPos(spinStartPos);
                 }
                 break;
 
             case SHOOT_CONTINOUS:
+                whileShooting = true;
                 ballPositions[0].isEmpty = false;
                 ballPositions[1].isEmpty = false;
                 ballPositions[2].isEmpty = false;
                 if (servos.getSpinPos() > spinEndPos){
+                    whileShooting = false;
+                    servos.setTransferPos(transferServo_out);
+                    shootTicks = 0;
                     currentIntakeState = IntakeState.FINDNEXT;
                 } else {
                     double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;

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

@@ -10,11 +10,9 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.constants.Color;
-import org.firstinspires.ftc.teamcode.constants.StateEnums;
 
 import java.util.List;
 
@@ -27,20 +25,18 @@ public class Turret {
     public static double turret180Range = 0.4;
     public static double turrDefault = 0.37;
     public static double turrMin = 0.15;
-    public static double turrMax = 0.85;
+    public static double turrMax = 0.8;
     public static boolean limelightUsed = true;
 
     public static double manualOffset = 0.0;
 
-    public static double visionCorrectionGain = 0.08;  // Single tunable gain
+//    public static double visionCorrectionGain = 0.08;  // Single tunable gain
-    public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
+//    public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
-    public static double cameraBearingEqual = 0.5;      // Deadband
+//    public static double cameraBearingEqual = 0.5;      // Deadband
 
-    // TODO: tune these values for limelight
+//    public static double clampTolerance = 0.03;
-
-    public static double clampTolerance = 0.03;
     //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
-    public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007;
+    public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
     Robot robot;
     MultipleTelemetry TELE;
     Limelight3A webcam;
@@ -49,7 +45,8 @@ public class Turret {
     double limelightPosX = 0.0;
     double limelightPosY = 0.0;
     LLResult result;
-
+    public static double TARGET_POSITION_TOLERANCE = 0.5;
+    public static double COLOR_OK_TOLERANCE = 2;
     boolean bearingAligned = false;
     private boolean lockOffset = false;
     private int obeliskID = 0;
@@ -57,12 +54,9 @@ public class Turret {
     private double currentTrackOffset = 0.0;
     private double lightColor = Color.LightRed;
     private int currentTrackCount = 0;
-    private double permanentOffset = 0.0;
+    double permanentOffset = 0.0;
     private int prevPipeline = -1;
-    private PIDController bearingPID;
+    PIDController bearingPID;
-
-    private double prevTurretPos = 0.0;
-    private boolean firstTurretPos = true;
 
     public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
         this.TELE = tele;
@@ -168,16 +162,16 @@ public class Turret {
     /*
         Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
      */
-
+    private double targetTx = 0;
+    public static double alphaTX = 0.5;
     private double bearingAlign(LLResult llResult) {
         double bearingOffset = 0.0;
-        double targetTx = llResult.getTx();   // How far left or right the target is (degrees)
+        double tx = llResult.getTx();   // How far left or right the target is (degrees)
-        final double MIN_OFFSET_POWER = 0.15;
+        targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
-        final double TARGET_POSITION_TOLERANCE = 1.0;
+//        final double MIN_OFFSET_POWER = 0.15;
-        // LL has 54.5 degree total Horizontal FOV; very edges are not useful.
+//        // 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 HORIZONTAL_FOV_RANGE = 26.0;  // Total usable horizontal degrees from center +/-
-        final double DRIVE_POWER_REDUCTION = 2.0;
+//        final double DRIVE_POWER_REDUCTION = 2.0;
-        final double COLOR_OK_TOLERANCE = 2.5;
 
         if (abs(targetTx) < TARGET_POSITION_TOLERANCE) {
             bearingAligned = true;
@@ -293,7 +287,7 @@ public class Turret {
         targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));
 
         // Interpolate towards target position
-        double currentPos = getTurrPos();
+//        double currentPos = getTurrPos();
         double turretPos = targetTurretPos;
 
         if (targetTurretPos == turrMin) {
@@ -303,7 +297,9 @@ public class Turret {
         }
 
         // Set servo positions
-        setTurret(turretPos + manualOffset);
+        if (!Spindexer.whileShooting || abs(targetTx) > COLOR_OK_TOLERANCE){
+            setTurret(turretPos + manualOffset);
+        }
 
 
         /* ---------------- TELEMETRY ---------------- */