Cleaning

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

@@ -13,6 +13,7 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Serializer;
+import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -42,8 +43,8 @@ public class RobotContainer {
   );
   */
   private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor);
-  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt);
-
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt, m_robotMap.serializerBeam);
+  private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -63,12 +64,15 @@ public class RobotContainer {
     //         getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
     m_robotIntake.setDefaultCommand(
-        new RunCommand(() -> m_robotIntake.runWithTriggers(
-          getDriverController().getLeftTriggerAxis(), getDriverController().getRightTriggerAxis()),m_robotIntake));
+      new RunCommand(() -> m_robotIntake.runWithTriggers(
+                                        getDriverController().getLeftTriggerAxis(),
+                                        getDriverController().getRightTriggerAxis()),
+                                        m_robotIntake));
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(
-        new RunCommand(m_robotLED::updateLED, m_robotLED));                      
-    // dri
+      new RunCommand(m_robotLED::updateLED, m_robotLED));                      
+    m_robotStorage.setDefaultCommand(
+      new RunCommand(m_robotStorage::manageStorage, m_robotStorage));
   }
 
   /**

src/main/java/frc4388/robot/RobotMap.java

@@ -8,10 +8,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SerializerConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
@@ -97,11 +99,16 @@ public class RobotMap {
   }
 
   /* Serializer Subsystem */
-
   public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
   public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
+  public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
   
   /* Intake Subsytem */
   public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
   public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
+
+  /* Storage Subsystem */
+  public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
+  public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
 }

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

@@ -12,10 +12,10 @@ public class Serializer extends SubsystemBase{
   private DigitalInput m_serializerBeam;
   private boolean serializerState;
 
-  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
+  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) {
     m_serializerBelt = serializerBelt;
     m_serializerShooterBelt = serializerShooterBelt;
-    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+    m_serializerBeam = serializerBeam;
 
     serializerState = false;
     setSerializerState(serializerState);

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

@@ -11,16 +11,18 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
-  public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
-  private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
-  private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+  public CANSparkMax m_storageMotor;
+  private DigitalInput m_beamShooter;
+  private DigitalInput m_beamIntake;
 
   /** Creates a new Storage. */
-  public Storage() {
-
+  public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) {
+    m_storageMotor = storageMotor;
+    m_beamShooter = beamShooter;
+    m_beamIntake = beamIntake;
   }
   public void manageStorage() {
-    if (m_beamShooter.get()) {
+    if (isBeamIntakeBroken()) { //Maybe needs to be shooter
       runStorage(1.d);
     } else { runStorage(0.d); }
   }
@@ -28,12 +30,12 @@ public class Storage extends SubsystemBase {
     m_storageMotor.set(input);
   }
 
-  public boolean getBeamShooter(){
-    return m_beamShooter.get();
+  public boolean isBeamShooterBroken(){
+    return !m_beamShooter.get();
   }
 
-  public boolean getBeamIntake(){
-    return m_beamIntake.get();
+  public boolean isBeamIntakeBroken(){
+    return !m_beamIntake.get();
   }
 
   @Override