Rework Command Constructors to Make Sense

Subsystems passed through the command are requirements, while subsystems gotten from other subsystems in the constructor are for reference only.

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

@@ -164,13 +164,13 @@ public class RobotContainer {
         /* Operator Buttons */
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
             .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
             //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
             .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
             //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)));
 
@@ -189,7 +189,7 @@ public class RobotContainer {
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage))
             .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
             //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
             //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))

src/main/java/frc4388/robot/commands/shooter/HoldTarget.java

@@ -41,9 +41,9 @@ public class HoldTarget extends CommandBase {
    * @param shooterSubsystem The Shooter subsystem
    * @param aimSubsystem The ShooterAim subsystem
    */
-  public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+  public HoldTarget(ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
-    m_shooter = shooterSubsystem;
+    m_shooter = m_shooterAim.m_shooterSubsystem;
     m_shooterHood = m_shooter.m_shooterHoodSubsystem;
     addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);

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

@@ -7,14 +7,13 @@
 
 package frc4388.robot.commands.shooter;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterHood;
 
 public class HoodPositionPID extends CommandBase {
+  ShooterHood m_shooterHood;
   double firingAngle;
-  private ShooterHood m_shooterHood;
+
   /**
    * Creates a new HoodPositionPID.
    */
@@ -49,7 +48,7 @@ public class HoodPositionPID extends CommandBase {
   public boolean isFinished() {
     double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition();
     if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
-      return false;
+      return true;
     }
     return false;
   }

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

@@ -25,13 +25,12 @@ public class ShootFireGroup extends ParallelRaceGroup {
    * @param m_shooterAim The ShooterAim subsystem
    * @param m_storage The Storage subsytem
    */
-  public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    ShooterHood m_shooterHood = m_shooter.m_shooterHoodSubsystem;
+  public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
 
     addCommands(
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
-      new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle())),
-      new HoldTarget(m_shooter, m_shooterAim),
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
+      new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
+      new HoldTarget(m_shooterAim),
       new StorageRun(m_storage)
     );
   }

src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java

@@ -10,6 +10,7 @@ package frc4388.robot.commands.shooter;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
 import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -22,10 +23,10 @@ public class ShootFullGroup extends SequentialCommandGroup {
    * @param m_shooterAim The ShooterAim subsystem
    * @param m_storage The Storage subsytem
    */
-  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
     addCommands(
-      new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
-      new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
+      new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), 
+      new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage)
     );
   }
 }

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

@@ -9,8 +9,10 @@ package frc4388.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.commands.storage.StoragePrepAim;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
 import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -23,12 +25,13 @@ public class ShootPrepGroup extends ParallelCommandGroup {
    * @param m_shooterAim The ShooterAim subsystem
    * @param m_storage The Storage subsytem
    */
-  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
     addCommands(
-      //new TrackTarget(m_shooter, m_shooterAim),
-      //new ShooterVelocityControlPID(m_shooter)
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()))
-      //new StoragePrepAim(m_storage)
+      new TrackTarget(m_shooterAim),
+      new ShooterVelocityControlPID(m_shooter),
+      new HoodPositionPID(m_shooterHood),
+      //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
+      new StoragePrepAim(m_storage)
     );
   }
 }

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

@@ -17,16 +17,14 @@ public class ShooterVelocityControlPID extends CommandBase {
   Shooter m_shooter;
   double m_targetVel;
   double m_actualVel;
-  private ShooterHood m_shooterHood;
   
   /**
    * Runs the drum at a velocity
    * @param subsystem The Shooter subsytem
    */
-  public ShooterVelocityControlPID(Shooter subsystem, ShooterHood subHood) {
+  public ShooterVelocityControlPID(Shooter subsystem) {
     m_shooter = subsystem;
-    m_shooterHood = subHood;
-    addRequirements(m_shooter, m_shooterHood);
+    addRequirements(m_shooter);
   }
 
   // Called when the command is initially scheduled.
@@ -39,7 +37,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   @Override
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
-    m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
+    //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
     //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
     //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
   }

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

@@ -19,9 +19,11 @@ import frc4388.utility.controller.IHandController;
 
 public class TrackTarget extends CommandBase {
   // Setup
-  NetworkTableEntry xEntry;
   ShooterAim m_shooterAim;
   Shooter m_shooter;
+  ShooterHood m_shooterHood;
+
+  NetworkTableEntry xEntry;
   IHandController m_driverController;
   // Aiming
   double turnAmount = 0;
@@ -34,16 +36,15 @@ public class TrackTarget extends CommandBase {
 
   public double m_hoodTrim;
   public double m_turretTrim;
-  private ShooterHood m_shooterHood;
 
   /**
    * Uses the Limelight to track the target
    * @param shooterSubsystem The Shooter subsystem
    * @param aimSubsystem The ShooterAim subsystem
    */
-  public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+  public TrackTarget(ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
-    m_shooter = shooterSubsystem;
+    m_shooter = m_shooterAim.m_shooterSubsystem;
     m_shooterHood = m_shooter.m_shooterHoodSubsystem;
     addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);