aaaaaaaaaaaa

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));
     }
        
     /**

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;
+  }
+}

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));
+  }
+}

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;
+  }
+}

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;
+  }
+}

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;
+  }
+}

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;
+  }
+}

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);
   }
 }