Full sequence possibly working, Full turret working

src/main/java/frc4388/robot/RobotContainer.java

@@ -62,6 +62,7 @@ import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.commands.StoragePrepAim;
 import frc4388.robot.commands.StoragePrepIntake;
+import frc4388.robot.commands.StorageRun;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.LimeLight;
@@ -180,12 +181,12 @@ public class RobotContainer {
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
-            .whileHeld(new ParallelCommandGroup(
-                new HoldTarget(m_robotShooter,m_robotShooterAim),
-                new HoodPositionPID(m_robotShooter)));
-            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
-            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            //.whileHeld(new ParallelCommandGroup(
+                //new HoldTarget(m_robotShooter,m_robotShooterAim),
+                //new HoodPositionPID(m_robotShooter)))
+            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)

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

@@ -13,12 +13,13 @@ import frc4388.robot.subsystems.Shooter;
 
 public class HoodPositionPID extends CommandBase {
   Shooter m_shooter;
+  double firingAngle;
   /**
    * Creates a new HoodPositionPID.
    */
   public HoodPositionPID(Shooter subSystem) {
     m_shooter = subSystem;
-    addRequirements(m_shooter);
+    //addRequirements(m_shooter);
   }
 
   // Called when the command is initially scheduled.
@@ -29,7 +30,7 @@ public class HoodPositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
+    firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
     SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
     SmartDashboard.putNumber("Fire Angle", firingAngle);
     m_shooter.runAngleAdjustPID(firingAngle);
@@ -43,6 +44,10 @@ public class HoodPositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
+    if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
+      return true;
+    }
     return false;
   }
 }

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

@@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
       new HoldTarget(m_shooter, m_shooterAim),
-      new StoragePositionPID(m_storage)
+      new StorageRun(m_storage)
     );
   }
 }

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

@@ -27,7 +27,8 @@ public class ShootPrepGroup extends ParallelCommandGroup {
     addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
-      new StoragePrepAim(m_storage)
+      new StoragePrepAim(m_storage),
+      new HoodPositionPID(m_shooter)
     );
   }
 }

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

@@ -36,8 +36,7 @@ public class StoragePrepIntake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_storage.getBeam(0));
-    if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
+    if (m_storage.getBeam(0)){
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -53,7 +52,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0) || startTime + 2000 <= System.currentTimeMillis()){
       return true;
     }
     return false;

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

@@ -29,12 +29,14 @@ public class StorageRun extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.err.println("oiudhgisbjkljasbhfkofhdnsekdfjbsjfvsdkcfbsdjhfgvsdkjfbsd");
     m_storage.runStorage(StorageConstants.STORAGE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_storage.runStorage(0);
   }
 
   // Returns true when the command should end.

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

@@ -83,8 +83,8 @@ public class TrackTarget extends CommandBase {
       double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
 
-      fireVel = Math.sqrt((Math.pow(xVel, 2)) + (Math.pow(yVel, 2)));
-      fireAngle = Math.atan(yVel / xVel) * (180/Math.PI);
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
     }
@@ -93,8 +93,7 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+
   }
 
   // Returns true when the command should end.