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;
@@ -35,22 +41,10 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
     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))

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;
@@ -70,12 +76,6 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double colorSenseTime = 1;
     public static double intakeStackTime = 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;

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;
 
@@ -51,10 +49,8 @@ public class AutoActions{
     private int mostGreenSlot = 0;
     private double firstSpindexShootPos = spindexer_outtakeBall1;
     private boolean shootForward = true;
-    public static double firstShootTime = 0.3;
     public int motif = 0;
     double spinEndPos = ServoPositions.spinEndPos;
-    int waitFirstBallTicks = 4;
 
     public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){
         this.robot = rob;
@@ -248,7 +244,7 @@ public class AutoActions{
                     end = prevSpinPos < spinEndPos;
                 }
 
-                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) {
+                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
 
                     if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
                         servos.setTransferPos(transferServo_out);
@@ -256,10 +252,10 @@ public class AutoActions{
                     } else {
                         servos.setTransferPos(transferServo_in);
                         shooterTicker++;
-
+                        Spindexer.whileShooting = true;
-                        if (shootForward && shooterTicker > waitFirstBallTicks) {
+                        if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
                             servos.setSpinPos(prevSpinPos + spindexSpeed);
-                        } else if (shooterTicker > waitFirstBallTicks){
+                        } else if (shooterTicker > Spindexer.waitFirstBallTicks){
                             servos.setSpinPos(prevSpinPos - spindexSpeed);
                         }
 
@@ -269,7 +265,7 @@ public class AutoActions{
 
                 } else {
                     servos.setTransferPos(transferServo_out);
-
+                    Spindexer.whileShooting = false;
                     spindexer.resetSpindexer();
                     spindexer.processIntake();
 
@@ -416,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);

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 {

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

@@ -5,11 +5,11 @@ 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;
@@ -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);
@@ -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/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,7 +60,7 @@ 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;
@@ -78,7 +77,6 @@ public class Flywheel {
         // really should be a running average of the last 5
         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;
@@ -35,7 +33,7 @@ public class Robot {
     public static double shooterPIDF_I = 0.0;
     public static double shooterPIDF_D = 0.0;
     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;
 
@@ -32,13 +30,11 @@ public class Turret {
 
     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.08, B_PID_I = 0.0, B_PID_D = 0.007;
     Robot robot;
@@ -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
+        if (!Spindexer.whileShooting || abs(targetTx) > COLOR_OK_TOLERANCE){
             setTurret(turretPos + manualOffset);
+        }
 
 
         /* ---------------- TELEMETRY ---------------- */