Shooter PREP

src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java

@@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Shooter;
 public class ShooterVelocityControlPID extends CommandBase {
   Shooter m_shooter;
   double m_targetVel;
+  double m_actualVel;
   /**
    * Creates a new ShooterVelocityControlPID.
    */
@@ -31,8 +32,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 
   // Called once the command ends or is interrupted.
@@ -43,6 +44,14 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    //Tells wether the target velocity has been reached
+    double upperBound = m_targetVel + 300;
+    double lowerBound = m_targetVel - 300;
+    if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      return true;
+    }
+    else{
+      return false;
+    }
   }
 }