Guys were def the 1st place aliance fr fr

src/main/java/frc/robot/RobotContainer.java

@@ -83,8 +83,8 @@ public class RobotContainer {
         m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
         m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
-        m_driverController.a().whileTrue(Commands.parallel(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)/*, intakeSubsystem.stopIntake(m_driverController.getHID())*/));
+        m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-        m_driverController.x().whileTrue(Commands.parallel(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)/*, intakeSubsystem.stopIntake(m_driverController.getHID())*/));
+        m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
         m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
         m_driverController.y().onTrue(intakeSubsystem.deploy());
     }

src/main/java/frc/robot/Utils.java

@@ -101,14 +101,14 @@ public class Utils {
     }
 
     public static double getHoodAngleForDistance(double distanceInMeters){
-        if(distanceInMeters >= 3){
+        if(distanceInMeters >= 4){
             return 200;
         }
         return 0;
     }
 
     public static double getLauncherRPMForDistance(double distanceInMeters){ 
-        return Math.min((667.557*distanceInMeters) + 1024.699, 6000);
+        return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
     }
 
     /**

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -149,7 +149,6 @@ public class DriveSubsystem extends TestableSubsystem {
 
                 Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
                 Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
-                Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
             }
 
         }, this));

src/main/java/frc/robot/subsystems/HopperSubsystem.java

@@ -17,6 +17,9 @@ import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {
@@ -86,6 +89,17 @@ public class HopperSubsystem extends TestableSubsystem {
         spindexerMotor.stopMotor();
     }
 
+    public Command spinny() {
+        return this.runOnce(() -> {
+            if (spindexerMotor.get() == 0) {
+                spindexerOn();
+            } else {
+                spindexerOff();
+            }
+        });
+    }
+
+
     @Override
     public void periodic() {
         Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -115,7 +115,7 @@ public class TurretSubsystem extends TestableSubsystem {
     SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
     config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
     config.voltageCompensation(10);
-    config.closedLoop.p(0.0001).i(0.000004).d(0.000007); // I = 0.0000001
+    config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
     config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000);
     // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
     config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
@@ -203,15 +203,8 @@ public class TurretSubsystem extends TestableSubsystem {
 
   @Override
   public void periodic() {
-    if(turretState == TurretState.HOMING)
+    if (turretState == TurretState.HOMING) {
-    {
+      if (turretLimitSwitch.get()) { // Turret has tripped the switch
-      while(turretState != TurretState.READY)
-      {
-        long start = System.currentTimeMillis();
-        while(System.currentTimeMillis()-1000<start)
-        {
-          if(turretLimitSwitch.get())
-          { // Turret has tripped the switch
         turretMotor.stopMotor();
         turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
         if (!Robot.isReal()) {
@@ -227,33 +220,28 @@ public class TurretSubsystem extends TestableSubsystem {
                 .withForwardSoftLimitEnable(true)
                 .withForwardSoftLimitThreshold(forwardLimit)
                 .withReverseSoftLimitEnable(true)
-                .withReverseSoftLimitThreshold(reverseLimit)
+                .withReverseSoftLimitThreshold(reverseLimit));
-            );
         // setTurretRobotRelative(0);
         // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
       }
     }
-        if(turretState != TurretState.READY)
-        {
-          turretMotor.set((turretMotor.get()*-1));
-        }
-      }
-    }
 
-    if(LOGGING)
+    if (LOGGING) {
-    {
       Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
       Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
       Logger.recordOutput("TurretSubsystem-TurretState", turretState);
       Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
       Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
-      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations));
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
-      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations));
+          turretMotorPosition.refresh().getValue().in(Units.Rotations));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
+          turretControlMode.getPositionMeasure().in(Units.Rotations));
       Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
       Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
       Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
       Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
       Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
+      Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
     }
 
   }
@@ -314,7 +302,7 @@ public class TurretSubsystem extends TestableSubsystem {
   }
 
   public boolean isTurretAtTarget(){
-    return turretPositionError.refresh().getValueAsDouble() <= 0.1;
+    return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
   }
 
   private double getRequestedPosTurretRelative() {