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 { * */ } +}