stopping after shooting and coasting

src/main/java/frc/robot/Constants.java

@@ -339,6 +339,7 @@ public static class FeederConstants {
         public static final int feederNoteSensorID = 0;
         public static final double FeederMotorSpeed = 0.8;
         public static final double getNoteSpeed = 0;
+        public static final double TimeToFeed = 0.7;
 
     }
 

src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java

@@ -6,15 +6,15 @@
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.basicCommands.ShooterArmCommands.ShooterArmSpeakerAngle;
-import frc.robot.basicCommands.ShooterCommands.ShooterSpeakerSetSpeed;
+import frc.robot.basicCommands.ShooterCommands.ShooterSpeaker;
 
 public class PrepareForShoot extends ParallelCommandGroup {
   /** Creates a new PrepareForShoot. */
   public PrepareForShoot() {
 
     addCommands(
       new ShooterArmSpeakerAngle(),
-      new ShooterSpeakerSetSpeed()
+      new ShooterSpeaker()
     );
   }
 }

src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java

@@ -4,10 +4,10 @@
 
 package frc.robot.aRobotOperations;
 
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import frc.robot.basicCommands.feederCommands.FeedToShooter;
 
-public class ShootSpeaker extends ParallelCommandGroup {
+public class ShootSpeaker extends ParallelRaceGroup {
   /** Creates a new ShootSpeaker. */
   public ShootSpeaker() {
     addCommands(

src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmSpeakerAngle.java

@@ -5,12 +5,12 @@
 package frc.robot.basicCommands.ShooterArmCommands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.Utils.vision.Vision;
 import frc.robot.subsystems.ShooterArmSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
 
 public class ShooterArmSpeakerAngle extends Command {
   private final ShooterArmSubsystem shooterArmSubsystem = ShooterArmSubsystem.getInstance();
-  private final Vision vision = Vision.getInstance();
+  private final SwerveSubsystem swerve = SwerveSubsystem.getInstance();
 
   /** Creates a new ShooterAngleFromDistanceInterpolation. */
   public ShooterArmSpeakerAngle() {
@@ -25,7 +25,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    shooterArmSubsystem.moveArmTo(shooterArmSubsystem.angleFromDistance(vision.DistanceFromTarget()));
+    shooterArmSubsystem.moveArmTo(shooterArmSubsystem.angleFromDistance(swerve.getPose()));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeakerSetSpeed.java → src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeaker.java

@@ -8,12 +8,12 @@
 import frc.robot.subsystems.ShooterSubsysem;
 import frc.robot.subsystems.SwerveSubsystem;
 
-public class ShooterSpeakerSetSpeed extends Command {
+public class ShooterSpeaker extends Command {
   private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance();
   private final SwerveSubsystem swerve = SwerveSubsystem.getInstance();
 
   /** Creates a new ShooterSetSpeedInterpolation. */
-  public ShooterSpeakerSetSpeed() {
+  public ShooterSpeaker() {
     this.addRequirements(shooterSubsystem);
   }
 
@@ -31,6 +31,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    shooterSubsystem.coast();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/basicCommands/feederCommands/FeedToShooter.java

@@ -4,16 +4,20 @@
 
 package frc.robot.basicCommands.feederCommands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.FeederConstants;
 import frc.robot.subsystems.FeederSubsystem;
 import frc.robot.subsystems.ShooterArmSubsystem;
 import frc.robot.subsystems.ShooterSubsysem;
 import static frc.robot.Constants.FeederConstants.*;
 
 public class FeedToShooter extends Command {
-  private final ShooterSubsysem shooterSubsysem = ShooterSubsysem.getInstance();
+  private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance();
   private final ShooterArmSubsystem shooterArmSubsystem = ShooterArmSubsystem.getInstance();
   private final FeederSubsystem feederSubsystem = FeederSubsystem.getInstance();
+  Timer timer = new Timer();
+  boolean startedShooting = false;
 
   /** Creates a new StartFeeder. */
   public FeedToShooter() {
@@ -29,8 +33,10 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (shooterSubsysem.checkIfShooterAtSpeed() && shooterArmSubsystem.isArmReady()) {
+    if (!startedShooting && shooterSubsystem.checkIfShooterAtSpeed() && shooterArmSubsystem.isArmReady()) {
       feederSubsystem.setSpeed(FeederShootSpeed);
+      startedShooting = true;
+      timer.restart();
     }
   }
 
@@ -43,6 +49,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return timer.hasElapsed(FeederConstants.TimeToFeed);
   }
 }

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

@@ -9,9 +9,12 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.Utils.interpolation.InterpolateUtil;
+import frc.Utils.vision.Vision;
 import frc.robot.Constants;
 
 import static frc.robot.Constants.ShooterArmConstants.*;
@@ -25,6 +28,7 @@ public class ShooterArmSubsystem extends SubsystemBase {
   private TalonFX m_shooterArmMotor;
   private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(shooterArmStartPose);
   DigitalInput limitSwitch;
+  private final Vision vision = Vision.getInstance();
 
   // the instance
   private static ShooterArmSubsystem instance;
@@ -109,8 +113,8 @@ public void moveArmBySpeed(DoubleSupplier speed) {
     m_shooterArmMotor.set(speed.getAsDouble());
   }
 
-  public double angleFromDistance(double distance) {
-    return InterpolateUtil.interpolate(SHOOTER_ANGLE_INTERPOLATION_MAP, distance);
+  public double angleFromDistance(Pose2d pose) {
+    return InterpolateUtil.interpolate(SHOOTER_ANGLE_INTERPOLATION_MAP, vision.DistanceFromTarget(pose));
   }
 
   @Override

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

@@ -8,7 +8,10 @@
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
 import static frc.robot.Constants.ShooterConstants.*;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -25,6 +28,8 @@ public class ShooterSubsysem extends SubsystemBase {
 
   private static ShooterSubsysem instance;
 
+  NeutralOut neutralOut = new NeutralOut();
+
   public static ShooterSubsysem getInstance() {
     if (instance == null) {
       instance = new ShooterSubsysem();
@@ -58,6 +63,8 @@ private ShooterSubsysem() {
     configs.Voltage.PeakReverseVoltage = PeakReverseVoltage;
 
     configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio;
+
+    m_shooterMotor.setNeutralMode(NeutralModeValue.Coast);
 
     // Checking if m_shooterMotor apply configs
     StatusCode status = StatusCode.StatusCodeNotInitialized;
@@ -88,6 +95,10 @@ public double speakerInterpolate(Pose2d pose2d) {
     return InterpolateUtil.interpolate(ShooterInterpolation, vision.DistanceFromTarget(pose2d));
   }
 
+  public void coast() {
+    m_shooterMotor.setControl(neutralOut);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run