tweaks were made and hardware changes are needed

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

@@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode {
         turret = new Turret(robot);
 
         parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
@@ -65,7 +66,7 @@ public class TeleopV4 extends LinearOpMode {
         turret.switchPipeline(Turret.PipelineMode.TRACKING);
         robot.limelight.start();
 
-        limelightUsed = false;
+        limelightUsed = true;
 
         waitForStart();
 
@@ -86,6 +87,20 @@ public class TeleopV4 extends LinearOpMode {
 
             follower.update();
 
+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(2500);
+                robot.setHoodPos(0.6);
+                robot.setTransferPower(-0.8);
+            }
+
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
@@ -110,12 +125,9 @@ public class TeleopV4 extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-            if (gamepad1.rightBumperWasPressed()
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
-                spindexerTransferIntake.setRapidMode(
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                        SpindexerTransferIntake.RapidMode.INTAKE
+                robot.setIntakePower(1);
-                );
             }
 
             if (gamepad2.leftBumperWasPressed()){

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

@@ -15,6 +15,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
 @TeleOp
@@ -57,6 +58,7 @@ public class NewShooterTest extends LinearOpMode {
         turret = new Turret(robot);
 
         parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -81,10 +83,28 @@ public class NewShooterTest extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            if (gamepad1.crossWasPressed()){
+                follower.setPose(TeleopV4.relocalizePose);
+                gamepad1.rumble(100);
+            }
+
             follower.update();
 
-            shooter.setFlywheelVelocity(flywheelVelo);
+            if (gamepad1.dpadLeftWasPressed()){
-            robot.setHoodPos(hoodPos);
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(flywheelVelo);
+                robot.setHoodPos(hoodPos);
+                robot.setTransferPower(transferPower);
+            }
+
+
 //            shooter.setTurretPosition(turretPos);
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
@@ -116,12 +136,8 @@ public class NewShooterTest extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-            if (gamepad1.rightBumperWasPressed()
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+                robot.setIntakePower(1);
-
-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.INTAKE
-                );
             }
 
             if (gamepad1.dpad_down){
@@ -135,6 +151,7 @@ public class NewShooterTest extends LinearOpMode {
             TELE.addData("Transfer Power", commander.getTransferPow());
             TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
             TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+            TELE.addData("TX:", turret.getTX());
 
             TELE.update();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -71,6 +71,10 @@ public class Shooter {
         this.state = shooterState;
     }
 
+    public ShooterState getState(){
+        return state;
+    }
+
     public void setTurretPosition(double input) {
         this.turretPosition = input;
     }

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

@@ -1,12 +1,10 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
-import static java.lang.Math.abs;
-
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
-import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.util.Range;
 
 
@@ -23,18 +21,18 @@ public class Turret {
     private final double turretMin = 0.05;
     private final double turretMax = 0.95;
     public static boolean limelightUsed = true;
-    public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
+    public static double B_PID_P = 0.0003, B_PID_I = 0.0, B_PID_D = 0.00003;
     LLResult result;
     PIDController bearingPID;
     boolean bearingAligned = false;
-    private boolean lockOffset = false;
     public int LL_COAST_TICKS = 60;
     public static double TARGET_POSITION_TOLERANCE = 0.5;
     public static double alphaTX = 0.5;
     private double targetTx = 0;
-    private double bearingOffset = 0;
+    private double currentTrackOffset = 0;
+    private double llCoast = 0;
+    private double servoAngle = 0.51;
     double tx = 0.0;
-    double ty = 0.0;
     private final double hVelK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
@@ -67,6 +65,8 @@ public class Turret {
         }
     }
 
+    public double getTX(){return tx;}
+
     public enum PipelineMode{
         OBELISK,
         TRACKING
@@ -137,13 +137,12 @@ public class Turret {
         robot.setTurretPos(pos);
     }
 
-    private double currentTrackOffset = 0;
-    private double llCoast = 0;
-    private double servoAngle = 0.51;
     public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
         // dx, dy, dz is target - robot
         // h is the raw heading where 0 degrees is positive x in the system of x, y
 
+        bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); // Keep when debugging/tuning, comment out when doing teleop
+
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
         double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
@@ -162,7 +161,7 @@ public class Turret {
                 targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
                 bearingAligned = Math.abs(targetTx) < TARGET_POSITION_TOLERANCE;
                 if (!bearingAligned){
-                    bearingOffset = -(bearingPID.calculate(targetTx, 0.0));
+                    bearingOffset = (bearingPID.calculate(targetTx, 0.0));
                 }
             } else {
                 targetTx = 0;
@@ -178,8 +177,6 @@ public class Turret {
             }
         }
 
-        currentTrackOffset += bearingOffset;
-
         double servoTicksFromNeutral = (angleDelta+currentTrackOffset) * (2.0 * servoTicksPer180);
 
         servoAngle = neutralPosition + servoTicksFromNeutral;