From 247a513e9ae1afc378466bf3360958f73ca29f02 Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Tue, 11 Feb 2020 17:35:16 -0700
Subject: [PATCH 01/12] Start shooter stuff

---
 build.gradle                                        |  2 +-
 src/main/java/frc4388/robot/subsystems/Drive.java   |  2 +-
 src/main/java/frc4388/robot/subsystems/Storage.java | 12 ++++++++++++
 3 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/build.gradle b/build.gradle
index e00a044..e0194f3 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.1.2"
+    id "edu.wpi.first.GradleRIO" version "2020.2.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 997f16e..f23bad4 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -101,7 +101,7 @@ public class Drive extends ProfiledPIDSubsystem {
     m_driveTrain.arcadeDrive(move, steer);
   }
 
-  public double getGyroYaw() {
+public double getGyroYaw() {
     double[] ypr = new double[3];
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[0];
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 7b85a0d..d3defc1 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -45,4 +45,16 @@ public class Storage extends SubsystemBase {
     m_storageMotor.set(input);
     boolean beam_on = m_beamSensors[0].get();
   }
+
+  /**
+   * Prepares storage for shooting
+   */
+  public void storageAim() {
+
+  }
+    /*
+    *If shooting move storage motor until top sensor is tripped
+    *If intaking move storage motor until bottom sensor is tripped
+    *
+    */
 }

From ee9bd4d3017fdcf5857798d3292dbdc1d5ba6873 Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Tue, 11 Feb 2020 20:07:35 -0700
Subject: [PATCH 02/12] Shooter gains fix

---
 src/main/java/frc4388/robot/Constants.java    |  9 ++-------
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../frc4388/robot/subsystems/Storage.java     | 20 ++++++++++---------
 3 files changed, 14 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index bcd15f8..318ac48 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -108,13 +108,8 @@ public final class Constants {
         public static final int PID_PRIMARY = 0;
 
         /* PID Gains */
-        public static final double storP = 0.1;
-        public static final double storI = 1e-4;
-        public static final double storD = 1.0;
-        public static final double storIz = 0.0;
-        public static final double storF = 0.0;
-        public static final double storkmaxOutput = 1.0;
-        public static final double storkminOutput = -1.0;
+        public static final double STORAGE_MIN_OUTPUT = -1.0;
+        public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
     }
   
     public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2585fbc..bf4a506 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -137,7 +137,7 @@ public class RobotContainer {
 
         /* Storage Neo PID Test */
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1)));
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5fbcd1c..be3f97f 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,7 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
@@ -30,6 +30,8 @@ public class Storage extends SubsystemBase {
 
   CANEncoder m_encoder = m_storageMotor.getEncoder();
 
+  Gains storageGains = StorageConstants.STORAGE_GAINS;
+
   /**
    * Creates a new Storage.
    */
@@ -72,15 +74,15 @@ public class Storage extends SubsystemBase {
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
+  public void runStoragePositionPID(double targetPos)
   {
     // Set PID Coefficients
-    m_storagePIDController.setP(kP);
-    m_storagePIDController.setI(kI);
-    m_storagePIDController.setD(kD);
-    m_storagePIDController.setIZone(kIz);
-    m_storagePIDController.setFF(kF);
-    m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kPeakOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
@@ -94,7 +96,7 @@ public class Storage extends SubsystemBase {
    * Prepares storage for shooting
    */
   public void storageAim() {
-
+    //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput);
   }
     /*
     *If shooting move storage motor until top sensor is tripped

From 1463bd209472d311be0b74d078e9ad772c0fde3b Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Thu, 13 Feb 2020 19:07:36 -0700
Subject: [PATCH 03/12] Add up and down for storage thingy

---
 .../java/frc4388/robot/RobotContainer.java    |  3 +++
 .../frc4388/robot/subsystems/Storage.java     | 19 +++++++++++++++++--
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bf4a506..5ad26e8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -138,6 +138,9 @@ public class RobotContainer {
         /* Storage Neo PID Test */
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake()));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index be3f97f..5f4d610 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -65,7 +65,7 @@ public class Storage extends SubsystemBase {
     } else {
       System.err.println("Beam off");
     }
-
+    
   }
 
   public void resetEncoder()
@@ -96,7 +96,21 @@ public class Storage extends SubsystemBase {
    * Prepares storage for shooting
    */
   public void storageAim() {
-    //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput);
+    if (m_beamSensors[0].get() == false){
+      m_storageMotor.set(0.5);
+    }
+    else{
+      m_storageMotor.set(0);
+    }
+  }
+
+public void storageIntake() {
+  if (m_beamSensors[2].get() == false){
+    m_storageMotor.set(-0.5);
+  }
+  else{
+    m_storageMotor.set(0);
+
   }
     /*
     *If shooting move storage motor until top sensor is tripped
@@ -104,3 +118,4 @@ public class Storage extends SubsystemBase {
     *
     */
 }
+}

From 0cf61e189337aa233ee5dc2d2041b3c52ad4dbc7 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Sat, 15 Feb 2020 11:16:12 -0800
Subject: [PATCH 04/12] Update Storage.java

---
 .../frc4388/robot/subsystems/Storage.java     | 33 +++++++++----------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5f4d610..280b3f9 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -32,6 +32,8 @@ public class Storage extends SubsystemBase {
 
   Gains storageGains = StorageConstants.STORAGE_GAINS;
 
+  Intake m_intake;
+
   /**
    * Creates a new Storage.
    */
@@ -56,17 +58,7 @@ public class Storage extends SubsystemBase {
    * 
    * @param input the voltage to run motor at
    */
-  public void runStorage(final double input) {
-    m_storageMotor.set(input);
-    final boolean beam_on = m_beamSensors[0].get();
-
-    if (beam_on) {
-      System.err.println("Beam on");
-    } else {
-      System.err.println("Beam off");
-    }
-    
-  }
+  
 
   public void resetEncoder()
   {
@@ -95,8 +87,8 @@ public class Storage extends SubsystemBase {
   /**
    * Prepares storage for shooting
    */
-  public void storageAim() {
-    if (m_beamSensors[0].get() == false){
+  public void storageAim() {  
+    if (m_beamSensors[2].get() == false){
       m_storageMotor.set(0.5);
     }
     else{
@@ -104,13 +96,21 @@ public class Storage extends SubsystemBase {
     }
   }
 
-public void storageIntake() {
-  if (m_beamSensors[2].get() == false){
+public void storageIntake(Intake intake) {
+  m_intake = intake;
+  if (m_beamSensors[1].get() == false){
     m_storageMotor.set(-0.5);
   }
   else{
     m_storageMotor.set(0);
-
+  }
+  if (m_beamSensors[0].get()){
+    m_intake.runExtender(-0.3);
+    m_storagePIDController.setReference(10, ControlType.kPosition);
+  }
+}
+public void storageOuttake() {
+     m_storageMotor.set(1);
   }
     /*
     *If shooting move storage motor until top sensor is tripped
@@ -118,4 +118,3 @@ public void storageIntake() {
     *
     */
 }
-}

From 57cc8a26e6fa921cb851a98332a710fb7bc1409b Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Sat, 15 Feb 2020 16:01:28 -0800
Subject: [PATCH 05/12] Even More Janky Code

Storage
---
 src/main/java/frc4388/robot/RobotContainer.java   | 10 +++++++---
 .../java/frc4388/robot/subsystems/Intake.java     | 12 ++++++++----
 .../java/frc4388/robot/subsystems/Storage.java    | 15 ++++++++-------
 3 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 5ad26e8..389c553 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -79,9 +79,8 @@ public class RobotContainer {
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter));
         // drives the leveler with an axis input from the driver controller
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
-        // runs storage motor at 50 percent
-        m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
     }
+      
 
     /**
     * Use this method to define your button->command mappings.  Buttons can be created by
@@ -139,8 +138,13 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
 
+        //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake()));
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake)));
+            
+        //Runs storage to outtake
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 62bb30a..679aac3 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -17,16 +17,20 @@ import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.IntakeConstants;
 
+
+
 public class Intake extends SubsystemBase {
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
-  CANDigitalInput m_extenderForwardLimit;
-  CANDigitalInput m_extenderReverseLimit;
+
 
   /**
    * Creates a new Intake.
    */
   public Intake() {
+    CANDigitalInput m_extenderForwardLimit;
+    CANDigitalInput m_extenderReverseLimit;
+
     m_intakeMotor.restoreFactoryDefaults();
     m_extenderMotor.restoreFactoryDefaults();
 
@@ -50,7 +54,7 @@ public class Intake extends SubsystemBase {
    * Runs intake motor
    * @param input the percent output to run motor at
    */
-  public void runIntake(double input) {
+  public void runIntake(final double input) {
     m_intakeMotor.set(input);
   }
   
@@ -58,7 +62,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender(double input) {
+  public void runExtender(final double input) {
     m_extenderMotor.set(input);
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 280b3f9..7c35066 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -9,12 +9,14 @@ package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.SparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
@@ -95,7 +97,6 @@ public class Storage extends SubsystemBase {
       m_storageMotor.set(0);
     }
   }
-
 public void storageIntake(Intake intake) {
   m_intake = intake;
   if (m_beamSensors[1].get() == false){
@@ -110,11 +111,11 @@ public void storageIntake(Intake intake) {
   }
 }
 public void storageOuttake() {
-     m_storageMotor.set(1);
+  m_storageMotor.set(1);
+  
+  /*
+  *If shooting move storage motor until top sensor is tripped
+  *If intaking move storage motor until bottom sensor is tripped
+  */
   }
-    /*
-    *If shooting move storage motor until top sensor is tripped
-    *If intaking move storage motor until bottom sensor is tripped
-    *
-    */
 }

From 86d6d5d946c548da2510d4458e1817f90bd6fe65 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Tue, 18 Feb 2020 18:12:06 -0800
Subject: [PATCH 06/12] Limit switches for extenders

Co-Authored-By: llamakingker <llamakingker@users.noreply.github.com>

#janky
---
 .../java/frc4388/robot/subsystems/Intake.java | 23 +++++++++++++++----
 .../frc4388/robot/subsystems/Storage.java     |  2 +-
 2 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 679aac3..63cb587 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -22,14 +22,15 @@ import frc4388.robot.Constants.IntakeConstants;
 public class Intake extends SubsystemBase {
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
-
+  CANDigitalInput m_extenderForwardLimit;
+  CANDigitalInput m_extenderReverseLimit;
+  boolean extended = false;
 
   /**
    * Creates a new Intake.
    */
   public Intake() {
-    CANDigitalInput m_extenderForwardLimit;
-    CANDigitalInput m_extenderReverseLimit;
+    
 
     m_intakeMotor.restoreFactoryDefaults();
     m_extenderMotor.restoreFactoryDefaults();
@@ -63,6 +64,18 @@ public class Intake extends SubsystemBase {
    * @param input the percent output to run motor at
    */
   public void runExtender(final double input) {
-    m_extenderMotor.set(input);
+    if (m_extenderForwardLimit.get()) {
+      extended = true;
+    }
+    if (m_extenderReverseLimit.get()) {
+      extended = false;
+    }
+    
+    if (extended == false) {
+      m_extenderMotor.set(0.5);
+    }
+    if (extended == true) {
+      m_extenderMotor.set(-0.5);
+    }
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 7c35066..db3d2fe 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -106,7 +106,7 @@ public void storageIntake(Intake intake) {
     m_storageMotor.set(0);
   }
   if (m_beamSensors[0].get()){
-    m_intake.runExtender(-0.3);
+    m_intake.runExtender(-0.3); 
     m_storagePIDController.setReference(10, ControlType.kPosition);
   }
 }

From d89f9da745ce7ab33e0bc3f7defb5c0b8488d084 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Tue, 18 Feb 2020 18:45:07 -0800
Subject: [PATCH 07/12] Changed Limit Switch on Extender

Co-Authored-By: llamakingker <llamakingker@users.noreply.github.com>

#janky
---
 .../robot/commands/RunExtenderOutIn.java      | 30 ++++++++++++++++---
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
index b0bb140..6cfd9c4 100644
--- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
@@ -7,14 +7,25 @@
 
 package frc4388.robot.commands;
 
+import com.revrobotics.CANDigitalInput;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.subsystems.Intake;
 import frc4388.utility.controller.IHandController;
 
 public class RunExtenderOutIn extends CommandBase {
   private Intake m_intake;
   private boolean isOut = false;
-  private long startTime;
+  CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
+  CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
+  CANDigitalInput m_extenderForwardLimit;
+  CANDigitalInput m_extenderReverseLimit;
+
+  
 
   /**
    * Uses input from opperator to run the extender motor.
@@ -25,13 +36,17 @@ public class RunExtenderOutIn extends CommandBase {
     // Use addRequirements() here to declare subsystem dependencies.
     m_intake = subsystem;
     addRequirements(m_intake);
+
+    m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderForwardLimit.enableLimitSwitch(false);
+    m_extenderReverseLimit.enableLimitSwitch(false);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     isOut = !isOut;
-    startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (startTime + 3000 < System.currentTimeMillis()) {
+    if (isOut && m_extenderForwardLimit.get() == true){
       return true;
     }
-    return false;
+    
+    else if(isOut && m_extenderReverseLimit.get() == true){
+      return true;
+    }
+    
+    else{
+      return false;
+    }
   }
 }

From 7667182b012dd0b87299ae6d142c50370528536e Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Thu, 20 Feb 2020 17:00:50 -0700
Subject: [PATCH 08/12] Changes to isExtended boolean

Refactored name according to boolean convention and combined two different booleans doing the same thing.
---
 .../java/frc4388/robot/commands/RunExtenderOutIn.java  | 10 +++++-----
 src/main/java/frc4388/robot/subsystems/Intake.java     | 10 +++++-----
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
index 6cfd9c4..3c062a0 100644
--- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
@@ -19,7 +19,7 @@ import frc4388.utility.controller.IHandController;
 
 public class RunExtenderOutIn extends CommandBase {
   private Intake m_intake;
-  private boolean isOut = false;
+
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
   CANDigitalInput m_extenderForwardLimit;
@@ -46,13 +46,13 @@ public class RunExtenderOutIn extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    isOut = !isOut;
+    m_intake.isExtended = !m_intake.isExtended;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (isOut){
+    if (m_intake.isExtended){
       m_intake.runExtender(0.3);
     } else {
       m_intake.runExtender(-0.3);
@@ -69,11 +69,11 @@ public class RunExtenderOutIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (isOut && m_extenderForwardLimit.get() == true){
+    if (m_intake.isExtended && m_extenderForwardLimit.get() == true){
       return true;
     }
     
-    else if(isOut && m_extenderReverseLimit.get() == true){
+    else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){
       return true;
     }
     
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 63cb587..d6b9b32 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -24,7 +24,7 @@ public class Intake extends SubsystemBase {
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
   CANDigitalInput m_extenderForwardLimit;
   CANDigitalInput m_extenderReverseLimit;
-  boolean extended = false;
+  public boolean isExtended = false;
 
   /**
    * Creates a new Intake.
@@ -65,16 +65,16 @@ public class Intake extends SubsystemBase {
    */
   public void runExtender(final double input) {
     if (m_extenderForwardLimit.get()) {
-      extended = true;
+      isExtended = true;
     }
     if (m_extenderReverseLimit.get()) {
-      extended = false;
+      isExtended = false;
     }
     
-    if (extended == false) {
+    if (isExtended == false) {
       m_extenderMotor.set(0.5);
     }
-    if (extended == true) {
+    if (isExtended == true) {
       m_extenderMotor.set(-0.5);
     }
   }

From 629046c0329094a6373f70ecb4ee3e2f8b006b44 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 20:41:36 -0700
Subject: [PATCH 09/12] aaaaaaaaaaaa

---
 .../java/frc4388/robot/RobotContainer.java    |  6 +-
 .../robot/commands/StorageIntakeFinal.java    | 46 +++++++++++++++
 .../robot/commands/StorageIntakeGroup.java    | 27 +++++++++
 .../frc4388/robot/commands/storageIntake.java | 58 +++++++++++++++++++
 .../frc4388/robot/commands/storageOutake.java | 44 ++++++++++++++
 .../robot/commands/storagePrepAim.java        | 52 +++++++++++++++++
 .../robot/commands/storagePrepIntake.java     | 56 ++++++++++++++++++
 .../frc4388/robot/subsystems/Storage.java     | 51 +++++-----------
 8 files changed, 300 insertions(+), 40 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
 create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
 create mode 100644 src/main/java/frc4388/robot/commands/storageIntake.java
 create mode 100644 src/main/java/frc4388/robot/commands/storageOutake.java
 create mode 100644 src/main/java/frc4388/robot/commands/storagePrepAim.java
 create mode 100644 src/main/java/frc4388/robot/commands/storagePrepIntake.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 26d1a69..ac85461 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -39,6 +39,7 @@ import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
 import frc4388.robot.commands.ShooterVelocityControlPID;
+import frc4388.robot.commands.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -49,6 +50,7 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Storage;
@@ -171,11 +173,11 @@ public class RobotContainer {
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake)));
+            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
+            .whileHeld(new storageOutake(m_robotStorage));
     }
        
     /**
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
new file mode 100644
index 0000000..9576f35
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class StorageIntakeFinal extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new StorageIntakeFinal.
+   */
+  public StorageIntakeFinal(Storage subsystem) {
+    m_storage = subsystem;
+    addRequirements(m_storage);
+  }
+
+  // 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() {
+    if (m_storage.getBeam(1)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 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;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
new file mode 100644
index 0000000..767ed69
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class StorageIntakeGroup extends SequentialCommandGroup {
+  /**
+   * Creates a new StorageIntakeGroup.
+   */
+  public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
+    addCommands(
+      new storagePrepIntake(m_intake, m_storage), 
+      new storageIntake(m_intake, m_storage), 
+      new StorageIntakeFinal(m_storage));
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/storageIntake.java
new file mode 100644
index 0000000..ee30708
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storageIntake.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storageIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storageIntake.
+   */
+  public storageIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // 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() {
+
+    if (m_storage.getBeam(0)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
+      m_intake.runExtender(-0.3); 
+    }
+    else{
+      m_intake.runExtender(0.3); 
+    }
+  }
+
+  // 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() {
+    if (m_storage.getBeam(0)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java
new file mode 100644
index 0000000..4820dc0
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storageOutake.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class storageOutake extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storageOutake.
+   */
+  public storageOutake(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // 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_storage.runStorage(-0.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;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java
new file mode 100644
index 0000000..c8b3bad
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storagePrepAim.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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class storagePrepAim extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storagePrepAim.
+   */
+  public storagePrepAim(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // 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() {
+    if (m_storage.getBeam(2) == false){
+      m_storage.runStorage(0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // 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() {
+    if (m_storage.getBeam(2)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
new file mode 100644
index 0000000..7fab016
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storagePrepIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storagePrepIntake.
+   */
+  public storagePrepIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // 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() {
+    if (m_storage.getBeam(1) == false){
+      m_storage.runStorage(-0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // 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() {
+    if (m_storage.getBeam(1)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index c71355a..24422c9 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,6 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
@@ -35,12 +36,14 @@ public class Storage extends SubsystemBase {
 
   Intake m_intake;
 
+  public boolean botReached;
+
   /**
    * Creates a new Storage.
    */
   public Storage() {
     resetEncoder();
-
+    boolean botReached = false;
     m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
     m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
     m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -65,14 +68,12 @@ public class Storage extends SubsystemBase {
     final boolean beam_on = m_beamSensors[0].get();
   }
 
-  public void resetEncoder()
-  {
+  public void resetEncoder(){
     m_encoder.setPosition(0);
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos)
-  {
+  public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
     m_storagePIDController.setP(storageGains.m_kP);
     m_storagePIDController.setI(storageGains.m_kI);
@@ -84,41 +85,15 @@ public class Storage extends SubsystemBase {
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
-  public void getEncoderPos()
-  {
-    m_encoder.getPosition();
+  public double getEncoderPos(){
+    return m_encoder.getPosition();
   }
 
-  /**
-   * Prepares storage for shooting
-   */
-  public void storageAim() {  
-    if (m_beamSensors[2].get() == false){
-      m_storageMotor.set(0.5);
-    }
-    else{
-      m_storageMotor.set(0);
-    }
+  public boolean getBeam(int id){
+    return m_beamSensors[id].get();
   }
-public void storageIntake(Intake intake) {
-  m_intake = intake;
-  if (m_beamSensors[1].get() == false){
-    m_storageMotor.set(-0.5);
-  }
-  else{
-    m_storageMotor.set(0);
-  }
-  if (m_beamSensors[0].get()){
-    m_intake.runExtender(-0.3); 
-    m_storagePIDController.setReference(10, ControlType.kPosition);
-  }
-}
-public void storageOuttake() {
-  m_storageMotor.set(1);
-  
-  /*
-  *If shooting move storage motor until top sensor is tripped
-  *If intaking move storage motor until bottom sensor is tripped
-  */
+
+  public void setStoragePID(double position){
+    m_storagePIDController.setReference(position , ControlType.kPosition);
   }
 }

From 80dfa95f79824de4db5755a5441dd772025aa746 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 20:51:09 -0700
Subject: [PATCH 10/12] fox

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index aed7e4e..5a7c7a0 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -64,7 +64,6 @@ public class Storage extends SubsystemBase {
   
   public void runStorage(final double input) {
     m_storageMotor.set(input);
-    final boolean beam_on = m_beamSensors[0].get();
   }
 
   public void resetEncoder(){
@@ -74,12 +73,12 @@ public class Storage extends SubsystemBase {
   /* Storage PID Control */
   public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
-    m_storagePIDController.setP(m_storageGains.m_kP);
-    m_storagePIDController.setI(m_storageGains.m_kI);
-    m_storagePIDController.setD(m_storageGains.m_kD);
-    m_storagePIDController.setIZone(m_storageGains.m_kIzone);
-    m_storagePIDController.setFF(m_storageGains.m_kF);
-    m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput);
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }

From d8e35e77df459adb8aa1a71cc6a8dfb5e06303a8 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 21:11:43 -0700
Subject: [PATCH 11/12] Requested reviews

---
 src/main/java/frc4388/robot/subsystems/Intake.java  | 4 ++--
 src/main/java/frc4388/robot/subsystems/Storage.java | 3 +--
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index d6b9b32..cbcd617 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -55,7 +55,7 @@ public class Intake extends SubsystemBase {
    * Runs intake motor
    * @param input the percent output to run motor at
    */
-  public void runIntake(final double input) {
+  public void runIntake(double input) {
     m_intakeMotor.set(input);
   }
   
@@ -63,7 +63,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender(final double input) {
+  public void runExtender() {
     if (m_extenderForwardLimit.get()) {
       isExtended = true;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5a7c7a0..3d460fe 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -42,7 +42,6 @@ public class Storage extends SubsystemBase {
    */
   public Storage() {
     resetEncoder();
-    boolean botReached = false;
     m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
     m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
     m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -62,7 +61,7 @@ public class Storage extends SubsystemBase {
    * @param input the voltage to run motor at
    */
   
-  public void runStorage(final double input) {
+  public void runStorage(double input) {
     m_storageMotor.set(input);
   }
 

From ac1bb02a7a28cabd0b9aff57f18670aca91bb7a4 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 21:18:12 -0700
Subject: [PATCH 12/12] Fix input for extender

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index cbcd617..6b716f4 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -63,7 +63,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender() {
+  public void runExtender(double input) {
     if (m_extenderForwardLimit.get()) {
       isExtended = true;
     }
@@ -72,10 +72,10 @@ public class Intake extends SubsystemBase {
     }
     
     if (isExtended == false) {
-      m_extenderMotor.set(0.5);
+      m_extenderMotor.set(input);
     }
     if (isExtended == true) {
-      m_extenderMotor.set(-0.5);
+      m_extenderMotor.set(-input);
     }
   }
 }
\ No newline at end of file