Merge branch 'button-fox' into highlanders-day

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

@@ -224,5 +224,6 @@ public final class Constants {
     public static final class OIConstants {
         public static final int XBOX_DRIVER_ID = 0;
         public static final int XBOX_OPERATOR_ID = 1;
+        public static final int BUTTON_FOX_ID = 2;
     }
 }

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

@@ -48,17 +48,22 @@ import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
+import frc4388.robot.commands.drive.PlaySongDrive;
 import frc4388.robot.commands.drive.TurnDegrees;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
+import frc4388.robot.commands.shooter.RunHoodWithJoystick;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
+import frc4388.robot.commands.shooter.ShooterGoalPosition;
+import frc4388.robot.commands.shooter.ShooterManual;
+import frc4388.robot.commands.shooter.ShooterTrenchPosition;
+import frc4388.robot.commands.shooter.ShooterVelocityControlPID;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
 import frc4388.robot.commands.storage.ManageStorage;
 import frc4388.robot.commands.storage.StoragePrep;
-import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
@@ -71,7 +76,10 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.ShooterHood;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Storage.StorageMode;
+import frc4388.utility.controller.ButtonFox;
 import frc4388.utility.controller.IHandController;
+import frc4388.utility.controller.JoystickManualButton;
 import frc4388.utility.controller.XboxController;
 
 /**
@@ -100,8 +108,10 @@ public class RobotContainer {
     private final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
-    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
-    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private static XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID);
+    private static XboxController m_manualXbox = new XboxController(3);
 
     /* Autos */
     double m_totalTimeAuto;
@@ -118,6 +128,7 @@ public class RobotContainer {
 
     TenBallAutoMiddle m_tenBallAutoMiddle;
 
+    public static boolean m_isShooterManual = false;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -148,6 +159,8 @@ public class RobotContainer {
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // runs the turret with joystick
         m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
+        // runs the hood with joystick
+        m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
         // moves the drum not
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter));
         // drives climber with input from triggers on the opperator controller
@@ -158,7 +171,7 @@ public class RobotContainer {
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
         //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
-        m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
+        m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
         //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
@@ -200,6 +213,9 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
             .whileHeld(new DisengageRachet(m_robotClimber));
 
+
+
+
         /* Operator Buttons */
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -250,22 +266,50 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
             .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
 
-        //Prepares storage for intaking
-        //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
-            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
-            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
-            //.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
-
-        //Runs storage to outtake
-       //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
-            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
-            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
 
         //Run drum
-        new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+        new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
             //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
+
+        //Run drum manual
+        new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true)
+            .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
+
+
+
+
+        /* Button Fox */
+        // Storage Manual
+        new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH)
+            .whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL)))
+            .whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET)));
+
+        // Meg
+        new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH)
+            .whenPressed(new PlaySongDrive(m_robotDrive))
+            .whenReleased(new InterruptSubystem(m_robotDrive));
+
+        // Shooter Manual
+        new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH)
+            .whileHeld(new ShooterManual(true))
+            .whenReleased(new ShooterManual(false));
+
+        // Goal Shooter Position
+        new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON)
+            .whileHeld(new ShooterGoalPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
+            .whenReleased(new InterruptSubystem(m_robotShooter))
+            .whenReleased(new InterruptSubystem(m_robotShooterHood))
+            .whenReleased(new InterruptSubystem(m_robotShooterAim));
+
+        // Trench Shooter Position
+        new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON)
+            .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
+            .whenReleased(new InterruptSubystem(m_robotShooter))
+            .whenReleased(new InterruptSubystem(m_robotShooterHood))
+            .whenReleased(new InterruptSubystem(m_robotShooterAim));
     }
 
     public void buildAutos() {
@@ -466,6 +510,12 @@ public class RobotContainer {
         return m_operatorXbox;
     }
 
+    public IHandController getButtonFoxObject()
+    {
+        return m_buttonFox;
+    }
+
+
     /**
      * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller.
      * Generic HIDs/Joysticks can be used to set up JoystickButtons.
@@ -485,4 +535,15 @@ public class RobotContainer {
     {
         return m_driverXbox.getJoyStick();
     }
+
+    /**
+     * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Button Fox.
+     * Generic HIDs/Joysticks can be used to set up JoystickButtons.
+     * @return The IHandController interface for the Button Fox.
+     */
+    public Joystick getButtonFox()
+    {
+        return m_buttonFox.getJoyStick();
+    }
+
 }

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

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.ShooterHood;
+import frc4388.utility.controller.IHandController;
+
+public class RunHoodWithJoystick extends CommandBase {
+  private ShooterHood m_hood;
+  private IHandController m_controller;
+
+  /**
+   * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
+   * @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) {
+    m_hood = subsystem;
+    m_controller = controller;
+    addRequirements(m_hood);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double input = m_controller.getRightYAxis();
+    m_hood.runHood(input);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
+
+public class ShooterGoalPosition extends CommandBase {
+  Shooter m_shooter;
+  ShooterHood m_hood;
+  ShooterAim m_aim;
+  /**
+   * Creates a new ShooterGoalPosition.
+   */
+  public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
+    m_shooter = shooterSub;
+    m_hood = hoodSub;
+    m_aim = aimSub;
+    addRequirements(m_shooter,m_hood,m_aim);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_shooter.runDrumShooterVelocityPID(5000);
+    m_hood.runAngleAdjustPID(3);
+    m_aim.runshooterRotatePID(-26.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+
+public class ShooterManual extends CommandBase {
+  public boolean isManual = false;
+  /**
+   * Creates a new ShooterManual.
+   */
+  public ShooterManual(boolean man) {
+    isManual = man;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    RobotContainer.m_isShooterManual = isManual;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.m_isShooterManual = isManual;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
+
+public class ShooterTrenchPosition extends CommandBase {
+  Shooter m_shooter;
+  ShooterHood m_hood;
+  ShooterAim m_aim;
+  /**
+   * Creates a new ShooterTrenchPosition.
+   */
+  public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
+    m_shooter = shooterSub;
+    m_hood = hoodSub;
+    m_aim = aimSub;
+    addRequirements(m_shooter,m_hood,m_aim);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_shooter.runDrumShooterVelocityPID(5000);
+    m_hood.runAngleAdjustPID(3);
+    m_aim.runshooterRotatePID(-26.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/storage/ManageStorage.java

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Storage.StorageMode;
 
 public class ManageStorage extends CommandBase {
   Storage m_storage;
@@ -26,17 +27,13 @@ public class ManageStorage extends CommandBase {
 
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
   boolean m_isStorageEmpty = true;
-
-  public enum StorageMode{IDLE, INTAKE, RESET};
-  StorageMode m_storageMode = StorageMode.IDLE;
   
   /**
    * Creates a new ManageStorage.
    */
-  public ManageStorage(Storage m_robotStorage, StorageMode storageMode) {
+  public ManageStorage(Storage m_robotStorage) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_storage = m_robotStorage;
-    m_storageMode = storageMode;
     addRequirements(m_storage);
   }
 
@@ -50,7 +47,7 @@ public class ManageStorage extends CommandBase {
 
     m_isStorageEmpty = !m_isBallInStorage;
 
-    if (m_storageMode == StorageMode.RESET) {
+    if (m_storage.m_storageMode == StorageMode.RESET) {
       m_resetStartTime = System.currentTimeMillis();
     }
   }
@@ -68,12 +65,14 @@ public class ManageStorage extends CommandBase {
     SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
     SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
 
-    if (m_storageMode == StorageMode.IDLE) {
+    if (m_storage.m_storageMode == StorageMode.IDLE) {
       runIdle();
-    } else if (m_storageMode == StorageMode.INTAKE) {
+    } else if (m_storage.m_storageMode == StorageMode.INTAKE) {
       runIntake();
-    } else if (m_storageMode == StorageMode.RESET) {
+    } else if (m_storage.m_storageMode == StorageMode.RESET) {
       runReset();
+    } else if (m_storage.m_storageMode == StorageMode.MANUAL) {
+      runManual();
     }
   }
 
@@ -91,10 +90,10 @@ public class ManageStorage extends CommandBase {
       }
       if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
         m_isStorageEmpty = false;
-        m_storageMode = StorageMode.IDLE;
+        m_storage.changeStorageMode(StorageMode.IDLE);
       }
     } else {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
   }
 
@@ -106,7 +105,7 @@ public class ManageStorage extends CommandBase {
     m_storage.runStorage(0);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
@@ -120,17 +119,24 @@ public class ManageStorage extends CommandBase {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
     } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
 
+  /**
+   * Switches Storage to Manual only
+   */
+  private void runManual() {
+    m_storage.runStorage(0);
+  }
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_storageMode = StorageMode.RESET;
+    m_storage.changeStorageMode(StorageMode.RESET);
   }
 
   // Returns true when the command should end.

src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Storage.StorageMode;
 
 public class ManageStoragePID extends CommandBase {
   Storage m_storage;
@@ -30,16 +31,13 @@ public class ManageStoragePID extends CommandBase {
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
   boolean m_isStorageEmpty = true;
 
-  public enum StorageMode{IDLE, INTAKE, RESET};
-  StorageMode m_storageMode = StorageMode.IDLE;
   
   /**
    * Creates a new ManageStorage.
    */
-  public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) {
+  public ManageStoragePID(Storage m_robotStorage) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_storage = m_robotStorage;
-    m_storageMode = storageMode;
     addRequirements(m_storage);
   }
 
@@ -55,7 +53,7 @@ public class ManageStoragePID extends CommandBase {
 
     m_intakeStartPos = m_storage.getEncoderPosInches();
 
-    if (m_storageMode == StorageMode.RESET) {
+    if (m_storage.m_storageMode == StorageMode.RESET) {
       m_resetStartTime = System.currentTimeMillis();
     }
   }
@@ -73,11 +71,11 @@ public class ManageStoragePID extends CommandBase {
     SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
     SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
 
-    if (m_storageMode == StorageMode.IDLE) {
+    if (m_storage.m_storageMode == StorageMode.IDLE) {
       runIdle();
-    } else if (m_storageMode == StorageMode.INTAKE) {
+    } else if (m_storage.m_storageMode == StorageMode.INTAKE) {
       runIntake();
-    } else if (m_storageMode == StorageMode.RESET) {
+    } else if (m_storage.m_storageMode == StorageMode.RESET) {
       runReset();
     }
   }
@@ -93,10 +91,10 @@ public class ManageStoragePID extends CommandBase {
 
       double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
       if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
-        m_storageMode = StorageMode.IDLE;
+        m_storage.changeStorageMode(StorageMode.IDLE);
       }
     } else {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
   }
 
@@ -108,7 +106,7 @@ public class ManageStoragePID extends CommandBase {
     m_storage.runStorage(0);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
       m_intakeStartPos = m_storage.getEncoderPosInches();
     }
     m_isStorageEmpty = !m_isBallInStorage;
@@ -123,10 +121,10 @@ public class ManageStoragePID extends CommandBase {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
       m_intakeStartPos = m_storage.getEncoderPosInches();
     } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
@@ -134,7 +132,7 @@ public class ManageStoragePID extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_storageMode = StorageMode.RESET;
+    m_storage.changeStorageMode(StorageMode.RESET);
   }
 
   // Returns true when the command should end.

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

@@ -88,6 +88,11 @@ public class ShooterHood extends SubsystemBase {
     m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
   }
 
+  public void runHood(double input)
+  {
+    m_angleAdjustMotor.set(input);
+  }
+
   public void resetGyroAngleAdj(){
     m_angleEncoder.setPosition(0);
 }

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

@@ -36,6 +36,9 @@ public class Storage extends SubsystemBase {
 
   public boolean m_isStorageReadyToFire = false;
 
+  public enum StorageMode{IDLE, INTAKE, RESET, MANUAL};
+  public StorageMode m_storageMode = StorageMode.IDLE;
+
   /**
    * Creates a new Storage.
    */
@@ -134,4 +137,8 @@ public class Storage extends SubsystemBase {
   public boolean getBeamIntake(){
     return m_beamIntake.get();
   }
+
+  public void changeStorageMode(StorageMode storageMode){
+    m_storageMode = storageMode;
+  }
 }