Fixes of intake and beams (untested)

src/main/java/frc4388/robot/commands/RunExtenderOutIn.java

@@ -38,9 +38,9 @@ public class RunExtenderOutIn extends CommandBase {
     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);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_extenderForwardLimit.enableLimitSwitch(true);
+    m_extenderReverseLimit.enableLimitSwitch(true);
   }
 
   // Called when the command is initially scheduled.

src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java

@@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase {
     double rightTrigger = m_controller.getRightTriggerAxis();
     double leftTrigger = m_controller.getLeftTriggerAxis();
     double output = 0;
-    if (leftTrigger < .5) {
-      if(leftTrigger > rightTrigger) {
-        output = leftTrigger;
-      }
-      if (rightTrigger > leftTrigger) {
-        output = -leftTrigger;
-      }
-    } else {
+    if(leftTrigger >= rightTrigger) {
       output = leftTrigger;
     }
+    if (rightTrigger > leftTrigger) {
+      output = -rightTrigger;
+    }
     m_intake.runIntake(output);
   }
 

src/main/java/frc4388/robot/commands/StorageIntake.java

@@ -39,7 +39,7 @@ public class StorageIntake extends CommandBase {
   @Override
   public void execute() {
 
-    if (!m_storage.getBeam(0)){
+    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }
@@ -56,7 +56,7 @@ public class StorageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(0) && intook){
+    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/StorageIntakeFinal.java

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+public class StorageIntakeFinal extends CommandBase {
+  /**
+   * Creates a new StorageIntakeFinal.
+   */
+  public StorageIntakeFinal() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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() {
+  }
+
+  // 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/StoragePrepIntake.java

@@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(0)){
+    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -54,7 +54,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
+    if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       return true;
     }
     return false;