From 60a896c97d60db03d21887aed06471263eeb49d0 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 5 Mar 2022 11:06:22 -0700
Subject: [PATCH 1/3] restore javadocs

---
 .../java/frc4388/robot/subsystems/Intake.java | 73 ++++++++++++++++++
 .../frc4388/robot/subsystems/Serializer.java  | 75 +++++++++++++++++++
 .../frc4388/robot/subsystems/Storage.java     | 61 +++++++++++++++
 3 files changed, 209 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java
 create mode 100644 src/main/java/frc4388/robot/subsystems/Serializer.java
 create mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
new file mode 100644
index 0000000..56c80b8
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+//Imported Limit switch ONLY
+import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.SparkMaxLimitSwitch.Type;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+
+public class Intake extends SubsystemBase {
+
+  private WPI_TalonFX m_intakeMotor;
+  private CANSparkMax m_extenderMotor;
+  private SparkMaxLimitSwitch m_inLimit;
+  private SparkMaxLimitSwitch m_outLimit;
+
+  public boolean toggle;
+
+  /** Creates a new Intake. */
+  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) {
+    m_intakeMotor = intakeMotor;
+    m_extenderMotor = extenderMotor; 
+
+    m_extenderMotor.restoreFactoryDefaults();
+
+    m_intakeMotor.setNeutralMode(NeutralMode.Brake);
+    m_intakeMotor.setInverted(false);
+    m_extenderMotor.setInverted(true);
+    
+    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_inLimit.enableLimitSwitch(true);
+    m_outLimit.enableLimitSwitch(true);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+  /**
+   * Runs The Intake With Triggers.
+   * @param leftTrigger Left Trigger to Run -
+   * @param rightTrigger Right Trigger to Run +
+   */
+  public void runWithTriggers(double leftTrigger, double rightTrigger) {
+    m_intakeMotor.set(rightTrigger - leftTrigger);
+  }
+  /**
+   * Runs The Extender
+   * @param extended Wether the Extender Is Extended
+   */
+  public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
+    double extenderMotorSpeed = extended ? 0.25d : 0.d;
+    m_extenderMotor.set(extenderMotorSpeed);
+  }
+  /**
+   * Toggles The Extender
+  */
+  public void toggleExtender() {
+    toggle = !toggle;
+    runExtender(toggle);
+  }
+  //Test
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
new file mode 100644
index 0000000..a3cf25e
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Serializer.java
@@ -0,0 +1,75 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants;
+import edu.wpi.first.wpilibj.DigitalInput;
+import com.revrobotics.CANSparkMax;
+
+public class Serializer extends SubsystemBase{
+  private CANSparkMax m_serializerBelt;
+  private CANSparkMax m_serializerShooterBelt;
+  private DigitalInput m_serializerBeam;
+  private boolean serializerState;
+
+  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
+    m_serializerBelt = serializerBelt;
+    m_serializerShooterBelt = serializerShooterBelt;
+    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
+
+    serializerState = false;
+    setSerializerState(serializerState);
+    m_serializerBelt.set(0);
+    m_serializerShooterBelt.set(0);
+    
+  }
+  /**
+   * Gets The State Of The Beam
+   * @return The State Of The Beam
+   */
+  public boolean getBeam() {
+    return m_serializerBeam.get();
+  }
+  /**
+   * Sets The Serializer State With The Beam
+   * @param state Your State Of The Button
+   * @param beambroken The State of the Beam Senser
+   */
+  public void setSerializerStateWithBeam(boolean state, boolean beambroken) {
+    boolean total = state || beambroken;
+    setSerializerState(total);
+  }
+  /**
+   * Sets The Serializer State With The Beam 
+   * @param state Your State Of The Button
+   */
+  public void setSerializerState(boolean state) {
+    setSerializerBeltState(state);
+    setSerializerShooterBeltState(state);
+    serializerState = state;
+  }
+  /**
+   * Sets the Serializer Belt State
+   * @param state Your State Of The Button
+   */
+  public void setSerializerBeltState(boolean state) {
+    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
+    m_serializerBelt.set(serializerBeltSpeed);
+  }
+  /**
+   * Sets the Shooter Belt State
+   * @param state Your State Of The Button
+   */
+  public void setSerializerShooterBeltState(boolean state) {
+    double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
+    m_serializerShooterBelt.set(serializerShooterBeltSpeed);
+  }
+  
+  /**
+   * Gets the Serializer State
+   * @return The Serializer State
+   */
+  public boolean getSerializerState() {
+    return serializerState;
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
new file mode 100644
index 0000000..a922e27
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+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);
+
+  /** Creates a new Storage. */
+  public Storage() {
+
+  }
+  /**
+   * If The Beam Is Broken, Run Storage
+   * If Else, Stop Running Storage
+   */
+  public void manageStorage() {
+    if (m_beamShooter.get()) {
+      runStorage(1.d);
+    } else { runStorage(0.d); }
+  }
+  /**
+   * Runs The Storage at a Specifyed Speed
+   * @param input The Specifyed Speed
+   */
+  public void runStorage(double input) {
+    m_storageMotor.set(input);
+  }
+  /**
+   * Gets The Beam State On The Shooter
+   * @return The State Of The Beam on the Shooter
+   */
+  public boolean getBeamShooter(){
+    return m_beamShooter.get();
+  }
+
+  /**
+   * Gets The Beam State Of The Intake
+   * @return The Beam State Of The Intake
+   */
+  public boolean getBeamIntake(){
+    return m_beamIntake.get();
+  }
+
+  @Override
+  /**
+   * Every Robot Tick Manage The Storage
+   */
+  public void periodic() {
+    manageStorage();
+  }
+}

From f895352bb922b0ba66b92f48bde1cc7b3050fa16 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 5 Mar 2022 11:07:54 -0700
Subject: [PATCH 2/3] DummySensor class, shoot is tested

---
 src/main/java/frc4388/robot/Constants.java    |  3 +-
 .../java/frc4388/robot/RobotContainer.java    | 24 +++---
 src/main/java/frc4388/robot/RobotMap.java     |  8 +-
 .../java/frc4388/robot/commands/Shoot.java    | 77 ++++++++-----------
 .../frc4388/robot/subsystems/BoomBoom.java    |  8 +-
 .../java/frc4388/robot/subsystems/Hood.java   | 44 +++++------
 .../robot/subsystems/SwerveModule.java        |  6 +-
 .../java/frc4388/robot/subsystems/Turret.java | 12 +--
 .../java/frc4388/utility/DummySensor.java     | 74 ++++++++++++++++++
 src/main/java/frc4388/utility/Gains.java      | 48 ++++++------
 10 files changed, 185 insertions(+), 119 deletions(-)
 create mode 100644 src/main/java/frc4388/utility/DummySensor.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 9600169..bd8cef4 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -142,7 +142,8 @@ public final class Constants {
     public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
     public static final double ENCODER_TICKS_PER_REV = 2048;
     
-    
+    // Shoot Command Constants
+    public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
    
     /* Turret Constants */
     //ID
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index dd05ec5..5c5b4ca 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -58,6 +58,7 @@ import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.commands.Shoot;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.LED;
@@ -88,12 +89,12 @@ public class RobotContainer {
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   private final Hood m_robotHood = new Hood();
-  // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
-  // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
+  private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
+  private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
-
+  
   /* Autonomous */
   private PathPlannerTrajectory loadedPathTrajectory = null;
   private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
@@ -119,7 +120,7 @@ public class RobotContainer {
 
     //Turret default command
 
-    // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
+    m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
 
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
@@ -161,17 +162,16 @@ public class RobotContainer {
 
     /* Operator Buttons */
     // activates "Lit Mode"
-    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+      .whenPressed(new InstantCommand());
+
     // activates "BoomBoom"
-    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-        .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
-        .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
-    // activates hood
-    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-        .whenPressed(() -> m_robotHood.runHood(0.5d))
-        .whenReleased(() -> m_robotHood.runHood(0.d));
+    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 649fcb2..3c85082 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -136,7 +136,7 @@ public class RobotMap {
   public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
   public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
 
-  // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
+  public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
 
   // Create motor CANSparkMax
   void configureShooterMotorControllers() {
@@ -163,9 +163,9 @@ public class RobotMap {
     shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
     
-    /* Turret Subsytem */
-    shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore
-    shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore
+    // /* Turret Subsytem */
+    // shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore
+    // shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore
 
   }
 
diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java
index 8fc6500..be04921 100644
--- a/src/main/java/frc4388/robot/commands/Shoot.java
+++ b/src/main/java/frc4388/robot/commands/Shoot.java
@@ -4,12 +4,17 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.hal.simulation.SimulatorJNI;
+import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
+import frc4388.utility.DummySensor;
+import frc4388.utility.Gains;
 
 public class Shoot extends CommandBase {
 
@@ -34,17 +39,24 @@ public class Shoot extends CommandBase {
   // pid
   public double error;
   public double prevError;
+  public Gains shootGains = ShooterConstants.SHOOT_GAINS;
   public double kP, kI, kD;
   public double proportional, integral, derivative;
   public double time;
   public double output;
   public double tolerance = 5.0;
 
-  // // dummy motor
-  // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420);
-  // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration();
+  // testing
+  public DummySensor dummy = new DummySensor(0);
 
-  /** Creates a new Shoot. */
+  /**
+   * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
+   * TODO: Velocity Correction
+   * @param sDrive Drive Train
+   * @param sShooter Shooter Drum
+   * @param sTurret Shooter Turret
+   * @param sHood Shooter Hood
+   */
   public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_swerve = sDrive;
@@ -54,14 +66,16 @@ public class Shoot extends CommandBase {
     
     addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
 
-    kP = 0.1;
-    kI = 0.0;
-    kD = 0.0;
+    kP = shootGains.kP;
+    kI = shootGains.kI;
+    kD = shootGains.kD;
 
     proportional = 0;
     integral = 0;
     derivative = 0;
     time = 0.02;
+
+    DummySensor.resetAll();
   }
 
   /**
@@ -83,8 +97,13 @@ public class Shoot extends CommandBase {
     // get targets (shooter tables)
     m_targetVel = m_boomBoom.getVelocity(m_distance);
     m_targetHood = m_boomBoom.getHood(m_distance);
+
+    // target angle tests
+    m_gyroAngle = 0;
+    m_odoX = -1;
+    m_odoY = 1;
+
     m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
-    m_driveTargetAngle = m_swerve.getRegGyro().getDegrees();
 
     // deadzone processing
     if (AimToCenter.isHardwareDeadzone(m_targetAngle)) {
@@ -93,37 +112,14 @@ public class Shoot extends CommandBase {
 
     if (AimToCenter.isDigitalDeadzone(m_targetAngle)) {
       // this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work
-      m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle + 20), Math.sin(m_driveTargetAngle + 20), true);
+      m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true);
     }
-
-    // // normal (i think) PID stuff
-    // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice();
-    // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID();
-    // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
-
-    // dummyConfiguration.slot0.kP = 0.1;
-    // dummyConfiguration.slot0.kI = 0;
-    // dummyConfiguration.slot0.kD = 0;
-    // dummyConfiguration.slot0.kF = 0;
-
-    // // weird PID stuff 
-    // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice();
-    // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID;
-    // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
-    // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0;
-  
-    // dummyConfiguration.slot1.kP = 0.1;
-    // dummyConfiguration.slot1.kI = 0;
-    // dummyConfiguration.slot1.kD = 0;
-    // dummyConfiguration.slot1.kF = 0;
-
-    // dummy.configAllSettings(dummyConfiguration);
-
+    
     // initial error
     updateError();
+    System.out.println("Error: " + error);
     prevError = error;
   }
-
   /**
    * Run custom PID.
    */
@@ -140,17 +136,12 @@ public class Shoot extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // dummy.selectProfileSlot(0, 0);
-    // dummy.selectProfileSlot(1, 1);
-    // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle);
-    // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true);
-    // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch
 
     // custom pid
     runPID();
-    m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
-                                                 // entire swerve drive or its the commented line below
-    // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
+    // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
+                                                 // entire swerve drive or its the line below
+    m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
     
     m_hood.runAngleAdjustPID(m_targetHood);
     m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
index c02de5e..e1286e5 100644
--- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java
+++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
@@ -181,10 +181,10 @@ public class BoomBoom extends SubsystemBase {
 
   public void setShooterGains() {
     m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
   public void runDrumShooterVelocityPID(double targetVel) {
diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java
index 5b1d2ff..c9e963f 100644
--- a/src/main/java/frc4388/robot/subsystems/Hood.java
+++ b/src/main/java/frc4388/robot/subsystems/Hood.java
@@ -21,31 +21,31 @@ import frc4388.utility.Gains;
 public class Hood extends SubsystemBase {
   public BoomBoom m_shooterSubsystem;
   
-  // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
   public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
   public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
   public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
-  // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+  public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
 
-  // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+  public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
   
   
-  //public boolean m_isHoodReady = false;
+  public boolean m_isHoodReady = false;
 
 public double m_fireAngle;
   
 
   /** Creates a new Hood. */
   public Hood() {
-    //  m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+     m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
 
-    // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_hoodUpLimitSwitch.enableLimitSwitch(true);
-    // m_hoodDownLimitSwitch.enableLimitSwitch(true);
+    m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodUpLimitSwitch.enableLimitSwitch(true);
+    m_hoodDownLimitSwitch.enableLimitSwitch(true);
 
-    // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
-    // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
     setHoodSoftLimits(true);
   }
     
@@ -60,30 +60,30 @@ public double m_fireAngle;
    * @param set Boolean to set soft limits to.
    */
   public void setHoodSoftLimits(boolean set) {
-    // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
-    // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
   }
 
   public void runAngleAdjustPID(double targetAngle) 
   {
     //Set PID Coefficients
-    // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
-    // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
-    // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
-    // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
-    // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
-    // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
+    m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP);
+    m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI);
+    m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD);
+    m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone);
+    m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF);
+    m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput);
 
-    // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
+    m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
   }
   
 
   public void runHood(double input) {
-    // m_angleAdjusterMotor.set(input);
+    m_angleAdjusterMotor.set(input);
   }
 
   public void resetGyroAngleAdj(){
-    // m_angleEncoder.setPosition(0);
+    m_angleEncoder.setPosition(0);
   }
 
   public double getAnglePosition(){
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 44afb70..d72199b 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -47,9 +47,9 @@ public class SwerveModule extends SubsystemBase {
 
     TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
 
-    angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP;
-    angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI;
-    angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD;
+    angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
+    angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
+    angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
 
     // Use the CANCoder as the remote sensor for the primary TalonFX PID
     angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java
index 3a00f02..7a0f45e 100644
--- a/src/main/java/frc4388/robot/subsystems/Turret.java
+++ b/src/main/java/frc4388/robot/subsystems/Turret.java
@@ -63,12 +63,12 @@ public class Turret extends SubsystemBase {
 
     m_boomBoomRotateMotor.setInverted(false);
 
-    m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
-    m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
-    m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
-    m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
-    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
-    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
+    m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
+    m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
+    m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
+    m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
+    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
+    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
   }
 
   @Override
diff --git a/src/main/java/frc4388/utility/DummySensor.java b/src/main/java/frc4388/utility/DummySensor.java
new file mode 100644
index 0000000..dadef80
--- /dev/null
+++ b/src/main/java/frc4388/utility/DummySensor.java
@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DummySensor {
+
+  private double value;
+  public double start;
+  public static ArrayList<DummySensor> instances = new ArrayList<DummySensor>();
+
+  /**
+   * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot.
+   * @param init The start "position" of the sensor (default is 0).
+   */
+  public DummySensor(double init) {
+    value = init;
+    start = init;
+    instances.add(this);
+  }
+
+  /**
+   * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot.
+   */
+  public DummySensor() {
+    value = 0;
+    start = 0;
+    instances.add(this);
+  }
+
+  /**
+   * Reset the "position" of the DummySensor to its starting value.
+   */
+  public void reset() {
+    value = start;
+  }
+
+  /**
+   * Reset the "position" of the DummySensor to a given value.
+   * @param val The "position" to reset the DummySensor to.
+   */
+  public void reset(double val) {
+    value = val;
+  }
+
+  /**
+   * Reset all instances of DummySensor to their starting values.
+   */
+  public static void resetAll() {
+    for (DummySensor instance : instances) {
+      instance.reset();
+    }
+  }
+
+  /**
+   * Get the "position" of the DummySensor.
+   * @return The current "position".
+   */
+  public double get() {
+    return value;
+  }
+
+  /**
+   * Apply an input to the DummySensor, changing its "position".
+   * @param input The input to apply.
+   */
+  public void apply(double input) {
+    value = value + input;
+  }
+
+}
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index 930e34f..13144a4 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -6,14 +6,14 @@ package frc4388.utility;
 
 /** Add your docs here. */
 public class Gains {
-  public double m_kP;
-  public double m_kI;
-  public double m_kD;
-  public double m_kF;
-  public int m_kIzone;
-  public double m_kPeakOutput;
-  public double m_kmaxOutput;
-  public double m_kminOutput;
+  public double kP;
+  public double kI;
+  public double kD;
+  public double kF;
+  public int kIzone;
+  public double kPeakOutput;
+  public double kmaxOutput;
+  public double kminOutput;
 
   /**
    * Creates Gains object for PIDs
@@ -26,14 +26,14 @@ public class Gains {
    */
   public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
   {
-    m_kP = kP;
-    m_kI = kI;
-    m_kD = kD;
-    m_kF = kF;
-    m_kIzone = kIzone;
-    m_kPeakOutput = kPeakOutput;
-    m_kmaxOutput = m_kPeakOutput;
-    m_kminOutput = -m_kPeakOutput;
+    this.kP = kP;
+    this.kI = kI;
+    this.kD = kD;
+    this.kF = kF;
+    this.kIzone = kIzone;
+    this.kPeakOutput = kPeakOutput;
+    this.kmaxOutput = this.kPeakOutput;
+    this.kminOutput = -this.kPeakOutput;
   }
 
   /**
@@ -48,13 +48,13 @@ public class Gains {
    */
   public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
   {
-    m_kP = kP;
-    m_kI = kI;
-    m_kD = kD;
-    m_kF = kF;
-    m_kIzone = kIzone;
-    m_kminOutput = kMinOutput;
-    m_kmaxOutput = kMaxOutput;
-    m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
+    this.kP = kP;
+    this.kI = kI;
+    this.kD = kD;
+    this.kF = kF;
+    this.kIzone = kIzone;
+    this.kminOutput = kMinOutput;
+    this.kmaxOutput = kMaxOutput;
+    this.kPeakOutput = (Math.abs(this.kminOutput) > Math.abs(this.kmaxOutput)) ? Math.abs(this.kminOutput) : Math.abs(this.kmaxOutput);
   }
 }

From 3d6f9d606d3b435dfd6cc429966d65153d15932c Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 5 Mar 2022 11:09:03 -0700
Subject: [PATCH 3/3] Revert "Merge branch 'swerve' of
 https://github.com/Team4388/2022NoWayHome into swerve"

This reverts commit 172547161339b69eb61d2ffb107d4a906324365c, reversing
changes made to f895352bb922b0ba66b92f48bde1cc7b3050fa16.
---
 .../java/frc4388/robot/subsystems/Intake.java | 73 ------------------
 .../frc4388/robot/subsystems/Serializer.java  | 75 -------------------
 .../frc4388/robot/subsystems/Storage.java     | 61 ---------------
 3 files changed, 209 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java
 delete mode 100644 src/main/java/frc4388/robot/subsystems/Serializer.java
 delete mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
deleted file mode 100644
index 56c80b8..0000000
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ /dev/null
@@ -1,73 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-//Imported Limit switch ONLY
-import com.revrobotics.SparkMaxLimitSwitch;
-import com.revrobotics.SparkMaxLimitSwitch.Type;
-
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.IdleMode;
-
-public class Intake extends SubsystemBase {
-
-  private WPI_TalonFX m_intakeMotor;
-  private CANSparkMax m_extenderMotor;
-  private SparkMaxLimitSwitch m_inLimit;
-  private SparkMaxLimitSwitch m_outLimit;
-
-  public boolean toggle;
-
-  /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) {
-    m_intakeMotor = intakeMotor;
-    m_extenderMotor = extenderMotor; 
-
-    m_extenderMotor.restoreFactoryDefaults();
-
-    m_intakeMotor.setNeutralMode(NeutralMode.Brake);
-    m_intakeMotor.setInverted(false);
-    m_extenderMotor.setInverted(true);
-    
-    m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_inLimit.enableLimitSwitch(true);
-    m_outLimit.enableLimitSwitch(true);
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-  /**
-   * Runs The Intake With Triggers.
-   * @param leftTrigger Left Trigger to Run -
-   * @param rightTrigger Right Trigger to Run +
-   */
-  public void runWithTriggers(double leftTrigger, double rightTrigger) {
-    m_intakeMotor.set(rightTrigger - leftTrigger);
-  }
-  /**
-   * Runs The Extender
-   * @param extended Wether the Extender Is Extended
-   */
-  public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
-    double extenderMotorSpeed = extended ? 0.25d : 0.d;
-    m_extenderMotor.set(extenderMotorSpeed);
-  }
-  /**
-   * Toggles The Extender
-  */
-  public void toggleExtender() {
-    toggle = !toggle;
-    runExtender(toggle);
-  }
-  //Test
-}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java
deleted file mode 100644
index a3cf25e..0000000
--- a/src/main/java/frc4388/robot/subsystems/Serializer.java
+++ /dev/null
@@ -1,75 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants;
-import edu.wpi.first.wpilibj.DigitalInput;
-import com.revrobotics.CANSparkMax;
-
-public class Serializer extends SubsystemBase{
-  private CANSparkMax m_serializerBelt;
-  private CANSparkMax m_serializerShooterBelt;
-  private DigitalInput m_serializerBeam;
-  private boolean serializerState;
-
-  public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) {
-    m_serializerBelt = serializerBelt;
-    m_serializerShooterBelt = serializerShooterBelt;
-    m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM);
-
-    serializerState = false;
-    setSerializerState(serializerState);
-    m_serializerBelt.set(0);
-    m_serializerShooterBelt.set(0);
-    
-  }
-  /**
-   * Gets The State Of The Beam
-   * @return The State Of The Beam
-   */
-  public boolean getBeam() {
-    return m_serializerBeam.get();
-  }
-  /**
-   * Sets The Serializer State With The Beam
-   * @param state Your State Of The Button
-   * @param beambroken The State of the Beam Senser
-   */
-  public void setSerializerStateWithBeam(boolean state, boolean beambroken) {
-    boolean total = state || beambroken;
-    setSerializerState(total);
-  }
-  /**
-   * Sets The Serializer State With The Beam 
-   * @param state Your State Of The Button
-   */
-  public void setSerializerState(boolean state) {
-    setSerializerBeltState(state);
-    setSerializerShooterBeltState(state);
-    serializerState = state;
-  }
-  /**
-   * Sets the Serializer Belt State
-   * @param state Your State Of The Button
-   */
-  public void setSerializerBeltState(boolean state) {
-    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
-    m_serializerBelt.set(serializerBeltSpeed);
-  }
-  /**
-   * Sets the Shooter Belt State
-   * @param state Your State Of The Button
-   */
-  public void setSerializerShooterBeltState(boolean state) {
-    double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
-    m_serializerShooterBelt.set(serializerShooterBeltSpeed);
-  }
-  
-  /**
-   * Gets the Serializer State
-   * @return The Serializer State
-   */
-  public boolean getSerializerState() {
-    return serializerState;
-  }
-}
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
deleted file mode 100644
index a922e27..0000000
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ /dev/null
@@ -1,61 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-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);
-
-  /** Creates a new Storage. */
-  public Storage() {
-
-  }
-  /**
-   * If The Beam Is Broken, Run Storage
-   * If Else, Stop Running Storage
-   */
-  public void manageStorage() {
-    if (m_beamShooter.get()) {
-      runStorage(1.d);
-    } else { runStorage(0.d); }
-  }
-  /**
-   * Runs The Storage at a Specifyed Speed
-   * @param input The Specifyed Speed
-   */
-  public void runStorage(double input) {
-    m_storageMotor.set(input);
-  }
-  /**
-   * Gets The Beam State On The Shooter
-   * @return The State Of The Beam on the Shooter
-   */
-  public boolean getBeamShooter(){
-    return m_beamShooter.get();
-  }
-
-  /**
-   * Gets The Beam State Of The Intake
-   * @return The Beam State Of The Intake
-   */
-  public boolean getBeamIntake(){
-    return m_beamIntake.get();
-  }
-
-  @Override
-  /**
-   * Every Robot Tick Manage The Storage
-   */
-  public void periodic() {
-    manageStorage();
-  }
-}