Add reverse mode

- Add reverse mode of autoAlign

TODO:
-  Shoot between normal and reverse mode
- Use the robot
- Make the math in autoAlign

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -20,16 +20,28 @@ public class Shooter extends SubsystemBase {
   private TalonFX leftShooter;
   private TalonFX rightShooter;
 
+  private Limelight limelight;
+
+  // 0 = Stop
+  // 1 = Idle, no limelight
+  // 2 = limelight
+  // 3 = Shooting
+  private int shooterMode;
+
+  
   /** Creates a new Shooter. */
-  public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) {
+  public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) {
     leftShooter  = leftTalonFX;
     rightShooter = rightTalonFX;
 
+    this.limelight = limelight;
+
     leftShooter.setNeutralMode(NeutralModeValue.Coast);
     rightShooter.setNeutralMode(NeutralModeValue.Coast);
   }
 
   public void spin() {
+    shooterMode = 3;
     spin(ShooterConstants.SHOOTER_SPEED);
   }
 
@@ -39,16 +51,30 @@ public class Shooter extends SubsystemBase {
   }
 
   public void stop() {
+    shooterMode = 0;
     spin(0.d);
   }
 
   public void idle() {
-    spin(ShooterConstants.SHOOTER_IDLE);
+    if(limelight.isNearSpeaker()){
+      shooterMode = 2;
+      spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
+    }else{
+      shooterMode = 1;
+      spin(ShooterConstants.SHOOTER_IDLE);
+    }
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+
+    if(limelight.isNearSpeaker() && shooterMode == 0){
+      shooterMode = 1;
+      spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
+    }
+
+    SmartDashboard.putNumber("Shooter Speed mode", shooterMode);
     SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
     SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());