Try to make shooter work

src/main/java/frc4388/robot/Constants.java

@@ -114,13 +114,8 @@ public final class Constants {
         public static final int PID_PRIMARY = 0;
 
         /* PID Gains */
-        public static final double storP = 0.1;
-        public static final double storI = 1e-4;
-        public static final double storD = 1.0;
-        public static final double storIz = 0.0;
-        public static final double storF = 0.0;
-        public static final double storkmaxOutput = 1.0;
         public static final double storkminOutput = -1.0;
+        public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1);
     }
   
     public static final class LEDConstants {

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

@@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.Constants.ShooterConstants;
-import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Storage;
 
@@ -23,6 +22,7 @@ public class ShootShooter extends CommandBase {
   Storage m_storage;
   private long startTime;
   private int ballNum;
+  public boolean velReach = false;
   /**
    * Creates a new ShootAlll.
    */
@@ -30,35 +30,35 @@ public class ShootShooter extends CommandBase {
     ballNum = numBall;
     m_shooter = shootSub;
     m_storage = storeSub;
-    addRequirements(m_shooter);
-    addRequirements(m_storage);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    startTime = System.currentTimeMillis();
-    //new InstantCommand(() -> m_storage.storageAiming());
+    //new InstantCommand(() -> m_storage.storageAim());
+    velReach = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java
 
       //Aiming
-    if (startTime + 3000 > System.currentTimeMillis())
+    if (!m_shooter.velReached && velReach == false) //If the shooter is spooled
     {
-      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel());
+      startTime = System.currentTimeMillis();
     }
     
-    else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
-      new RunCommand(() -> m_storage.runStorage(0.5));
-      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
-      new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle()));
+    else {
+      velReach = true;
+      new ParallelCommandGroup(
+        new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
+        new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
+        new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum)))
+      );
     }
   }
-
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
@@ -67,7 +67,7 @@ public class ShootShooter extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
+    if (startTime + (1000 * ballNum) < System.currentTimeMillis()){
       return true;
     }
     return false;

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

@@ -49,12 +49,7 @@ public class TrackTarget extends CommandBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
-  public double addFireVel() {
-    return fireVel;
-  }
-  public double addFireAngle() {
-    return fireAngle;
-  }
+
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -81,6 +76,8 @@ public class TrackTarget extends CommandBase {
 
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
       fireAngle = Math.atan(yVel/xVel);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle;
     }
   }
 

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

@@ -48,6 +48,9 @@ public class Shooter extends SubsystemBase {
 
   double velP;
   double input;
+  public boolean velReached;
+  public double m_fireVel;
+  public double m_fireAngle;
   
   /*
    * Creates a new Shooter subsystem.
@@ -79,6 +82,13 @@ public class Shooter extends SubsystemBase {
     SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
   }
 
+  public double addFireVel() {
+    return m_fireVel;
+  }
+  public double addFireAngle() {
+    return m_fireAngle;
+  }
+
   /**
    * Runs drum shooter motor.
    * @param speed Speed to set the motor at
@@ -105,15 +115,26 @@ public class Shooter extends SubsystemBase {
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
     velP = actualVel/targetVel; //Percent of target
     double runSpeed = actualVel + (12000*velP); //Ramp up equation
-    //if (runSpeed > targetVel) {runSpeed = targetVel;}
     SmartDashboard.putNumber("shoot", actualVel);
     runSpeed = runSpeed/targetVel; //Convert to percent
+
     if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
       m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
     }
     else{ //PID Based on targetVel
       m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
     }
+
+      //Tells wether the target velocity has been reached
+    double upperBound = targetVel + 300;
+    double lowerBound = targetVel - 300;
+    if (actualVel < upperBound && actualVel > lowerBound)
+    {
+      velReached = true;
+    }
+    else{
+      velReached = false;
+    }
   }
 
 

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

@@ -18,7 +18,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
@@ -29,6 +29,8 @@ public class Storage extends SubsystemBase {
 
   CANEncoder m_encoder = m_storageMotor.getEncoder();
 
+  public static Gains m_storageGains = StorageConstants.STORAGE_GAINS;
+
   /**
    * Creates a new Storage.
    */
@@ -65,21 +67,21 @@ public class Storage extends SubsystemBase {
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
+  public void runStoragePositionPID(double targetPos)
   {
     // Set PID Coefficients
-    m_storagePIDController.setP(kP);
-    m_storagePIDController.setI(kI);
-    m_storagePIDController.setD(kD);
-    m_storagePIDController.setIZone(kIz);
-    m_storagePIDController.setFF(kF);
-    m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
+    m_storagePIDController.setP(m_storageGains.m_kP);
+    m_storagePIDController.setI(m_storageGains.m_kI);
+    m_storagePIDController.setD(m_storageGains.m_kD);
+    m_storagePIDController.setIZone(m_storageGains.m_kIzone);
+    m_storagePIDController.setFF(m_storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
-  public void getEncoderPos()
+  public double getEncoderPos()
   {
-    m_encoder.getPosition();
+    return m_encoder.getPosition();
   }
 }