Fix most OI JoystickButtons

src/main/java/org/whsrobotics/commands/MoveElevatorVelocity.java

@@ -14,10 +14,10 @@ public MoveElevatorVelocity() {
     @Override
     protected void execute() {
 
-        if (OI.leftXboxJoystickCurve(OI.getXboxController().getTriggerAxis(GenericHID.Hand.kRight)) > 0 && !Elevator.getTopLimitSwitch()) {
+        if (OI.leftXboxJoystickCurve(OI.getXboxController().getTriggerAxis(GenericHID.Hand.kRight)) > 0) {
             Elevator.moveWithVelocity(OI.getXboxController().getTriggerAxis(GenericHID.Hand.kRight));
 
-        } else if (OI.leftXboxJoystickCurve(OI.getXboxController().getTriggerAxis(GenericHID.Hand.kLeft)) > 0 && !Elevator.getBottomLimitSwitch()) {
+        } else if (OI.leftXboxJoystickCurve(OI.getXboxController().getTriggerAxis(GenericHID.Hand.kLeft)) > 0) {
             Elevator.moveWithVelocity(-OI.getXboxController().getTriggerAxis(GenericHID.Hand.kLeft));
         }
 
@@ -26,7 +26,6 @@ protected void execute() {
     @Override
     protected void end() {
         Elevator.moveToValue(Elevator.getEncoderPosition());   // Hold the current position
-        // Elevator.moveWithVelocity(0);
     }
 
     @Override

src/main/java/org/whsrobotics/commands/commandgroups/CGDeployCubeToExchange.java

@@ -22,7 +22,7 @@ public CGDeployCubeToExchange(){
         // Spin the CubeSpinner motors in the OUTWARDS mode (until Cube has left the IR sensor), and open arms TODO: TEST
         addSequential(new SpinCubeSpinner(CubeSpinner.Mode.OUTWARDS), 3);
 
-        addSequential(new MoveCubeGripper(CubeGripper.Position.CLOSE_ARMS));
+        addSequential(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE));
         addSequential(new SpinCubeSpinner(CubeSpinner.Mode.OFF));
 
     }

src/main/java/org/whsrobotics/commands/commandgroups/CGDeployCubeToScale.java

@@ -28,7 +28,7 @@ protected boolean isFinished() {
 
         addSequential(new TimedCommand(2));
 
-        addSequential(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE));
+        addSequential(new MoveCubeGripper(CubeGripper.Position.OPEN_ARMS));
         addSequential(new SpinCubeSpinner(CubeSpinner.Mode.OFF));
         addSequential(new MoveElevatorPosition(Elevator.Position.DOWN));
 

src/main/java/org/whsrobotics/commands/commandgroups/CGDeployCubeToSwitch.java

@@ -6,11 +6,14 @@
 import org.whsrobotics.subsystems.CubeGripper;
 import org.whsrobotics.subsystems.CubeSpinner;
 import org.whsrobotics.subsystems.Elevator;
+import org.whsrobotics.utils.AutomationCancelerHelper;
 
-public class CGDeployCubeToSwitch extends CommandGroup{
+public class CGDeployCubeToSwitch extends CommandGroup {
 
     public CGDeployCubeToSwitch(){
 
+        requires(new AutomationCancelerHelper());
+
         addSequential(new MoveElevatorPosition(Elevator.Position.SWITCH));
 
         addSequential(new Command() {
@@ -28,7 +31,7 @@ protected boolean isFinished() {
 
         addSequential(new TimedCommand(2));
 
-        addSequential(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE));
+        addSequential(new MoveCubeGripper(CubeGripper.Position.OPEN_ARMS));
         addSequential(new SpinCubeSpinner(CubeSpinner.Mode.OFF));
         addSequential(new MoveElevatorPosition(Elevator.Position.DOWN));
 

src/main/java/org/whsrobotics/commands/commandgroups/CGGrabCube.java

@@ -1,15 +1,13 @@
 package org.whsrobotics.commands.commandgroups;
 
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.whsrobotics.commands.*;
 import org.whsrobotics.subsystems.Arduino;
 import org.whsrobotics.subsystems.CubeGripper;
 import org.whsrobotics.subsystems.CubeSpinner;
 import org.whsrobotics.subsystems.Elevator;
-
-import java.util.concurrent.atomic.AtomicReference;
+import org.whsrobotics.utils.AutomationCancelerHelper;
 
 /**
  * Sequence of commands to grab a Power Cube. Opens the robot arms and spins the intake wheels.
@@ -18,6 +16,8 @@ public class CGGrabCube extends CommandGroup {
 
     public CGGrabCube() {
 
+        requires(new AutomationCancelerHelper());
+
         // Move the Elevator to the down position. If it can't do it in 3 seconds, stop it.
         addSequential(new MoveElevatorPosition(Elevator.Position.DOWN));
 
@@ -28,14 +28,14 @@ protected boolean isFinished() {
             }
         }, 3);
 
-        // Move the CubeGripper arms to the GRAB_CUBE position
-        addSequential(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE), 3); // Maximum 3 seconds
+        // Move the CubeGripper arms to the OPEN_ARMS position
+        addSequential(new MoveCubeGripper(CubeGripper.Position.OPEN_ARMS), 3); // Maximum 3 seconds
 
         // Wait until Cube is detected with the sensor (40 cm)
         addSequential(new ArduinoUltrasonicDistance(40));
 
-        // Move the CubeGripper arms to the CLOSE_ARMS position
-        addSequential(new MoveCubeGripper(CubeGripper.Position.CLOSE_ARMS), 3); // Maximum 3 seconds
+        // Move the CubeGripper arms to the GRAB_CUBE position
+        addSequential(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE), 3); // Maximum 3 seconds
 
         // Spin the CubeSpinner motors in the INTAKE mode (until Cube is detected with the sensor), and bring arms closer
         addSequential(new SpinCubeSpinner(CubeSpinner.Mode.INWARDS));

src/main/java/org/whsrobotics/robot/OI.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.whsrobotics.commands.*;
@@ -13,9 +14,9 @@
 import org.whsrobotics.commands.commandgroups.CGGrabCube;
 import org.whsrobotics.subsystems.CubeGripper;
 import org.whsrobotics.subsystems.CubeSpinner;
-import org.whsrobotics.subsystems.DriveTrain;
 import org.whsrobotics.subsystems.Elevator;
 import org.whsrobotics.triggers.ElevatorVelocityMode;
+import org.whsrobotics.utils.AutomationCanceler;
 import org.whsrobotics.utils.RobotLogger;
 
 import static org.whsrobotics.robot.RobotMap.BUTTONBOX_PORT;
@@ -38,25 +39,52 @@ private OI() {
         xboxController = new XboxController(XBOX_PORT);
         buttonBox = new Joystick(BUTTONBOX_PORT);
 
+        (new JoystickButton(xboxController, XboxButton.kB.value)).whenPressed(new DriveForward());  // TODO: Change command
+
         (new JoystickButton(buttonBox, 1)).whenPressed(new CGGrabCube());
         (new JoystickButton(buttonBox, 2)).whenPressed(new CGDeployCubeToSwitch());
         (new JoystickButton(buttonBox, 3)).whenPressed(new CGDeployCubeToScale());
         (new JoystickButton(buttonBox, 4)).whenPressed(new CGDeployCubeToExchange());
-        (new JoystickButton(buttonBox, 5));
-        (new JoystickButton(buttonBox, 6)).whenPressed(new MoveCubeGripper(CubeGripper.Position.GRAB_CUBE));
+        (new JoystickButton(buttonBox, 5)).whenPressed(new AutomationCanceler());   // TODO: Does this even work?
+
+        (new JoystickButton(buttonBox, 6)).whenPressed(new MoveCubeGripper(CubeGripper.Position.OPEN_ARMS));
         (new JoystickButton(buttonBox, 7)).whenPressed(new CubeGripperApplyConstantVoltage());
-        (new JoystickButton(buttonBox, 8)).whenPressed(new MoveCubeGripper(CubeGripper.Position.CLOSE_ARMS));
-        (new JoystickButton(buttonBox, 9)).whenPressed(new SpinCubeSpinner(CubeSpinner.Mode.INWARDS));
-        (new JoystickButton(buttonBox, 10)).whenPressed(new SpinCubeSpinner(CubeSpinner.Mode.OUTWARDS));
-        (new JoystickButton(buttonBox, 11)).whenPressed(new CubeGripper.disableOutputCommand());
-        (new JoystickButton(buttonBox, 12)).whenPressed(new SpinCubeSpinner(CubeSpinner.Mode.INWARDS));
-        (new JoystickButton(buttonBox, 13)).whenPressed(new ());
-        (new JoystickButton(buttonBox, 14)).whenPressed(new MoveElevatorPosition(Elevator.Position.SCALE_TOP));
-        (new JoystickButton(buttonBox, 15)).whenPressed(new MoveElevatorPosition(Elevator.Position.DOWN));
-            //FORCE VOLTAGE IS APPLY CONSTANT VOLTAGE
+        (new JoystickButton(buttonBox, 8)).whenPressed(new MoveCubeGripper(CubeGripper.Position.FOLD_BACK));
+
+        (new JoystickButton(buttonBox, 9)).whileHeld(new SpinCubeSpinner(CubeSpinner.Mode.INWARDS));
+        (new JoystickButton(buttonBox, 9)).whenReleased(new SpinCubeSpinner(CubeSpinner.Mode.OFF));
+        (new JoystickButton(buttonBox, 10)).whileHeld(new SpinCubeSpinner(CubeSpinner.Mode.OUTWARDS));
+        (new JoystickButton(buttonBox, 10)).whenReleased(new SpinCubeSpinner(CubeSpinner.Mode.OFF));
+
+        (new JoystickButton(buttonBox, 11)).whenPressed(CubeGripper.disableOutputCommand);  // TODO: May not work! Put in its own command.
+        (new JoystickButton(buttonBox, 12)).whileHeld(new Command() {
+            @Override
+            protected boolean isFinished() {
+                return false;
+            }
+        }); // Open Arms
+        (new JoystickButton(buttonBox, 13)).whileHeld(new Command() {
+            @Override
+            protected boolean isFinished() {
+                return false;
+            }
+        }); // Close Arms
+
+        (new JoystickButton(buttonBox, 14)).whileHeld(new Command() {
+            @Override
+            protected boolean isFinished() {
+                return false;
+            }
+        }); // Elevator Up
+        (new JoystickButton(buttonBox, 15)).whileHeld(new Command() {
+            @Override
+            protected boolean isFinished() {
+                return false;
+            }
+        }); // Elevator Down
 
 
-        (new ElevatorVelocityMode()).whenActive(new MoveElevatorVelocity());
+        (new ElevatorVelocityMode()).whenActive(new MoveElevatorVelocity());    // Convert to LT/RT?
 
         publishElevator();
         publishCubeSpinner();
@@ -227,7 +255,7 @@ public static CubeSpinner.Mode getSelectedCubeSpinnerMode() {
 
     private static void publishCubeGripper() {
         cubeGripperModeChooser = new SendableChooser<>();
-        cubeGripperModeChooser.addDefault("Default - SWITCH", CubeGripper.Position.GRAB_CUBE);
+        cubeGripperModeChooser.addDefault("Default - SWITCH", CubeGripper.Position.OPEN_ARMS);
 
         for (CubeGripper.Position position : CubeGripper.Position.values()) {
             cubeGripperModeChooser.addObject(position.toString(), position);

src/main/java/org/whsrobotics/subsystems/CubeGripper.java

@@ -17,11 +17,11 @@ public class CubeGripper extends Subsystem {
     private static TalonSRX right;
 
     public enum Position {
-        START_MATCH(0), GRAB_CUBE(1550), CLOSE_ARMS(2048);  // Native Encoder Units // 1 complete rotation = 4096 ticks
+        FOLD_BACK(0), OPEN_ARMS(1550), GRAB_CUBE(2048);  // Native Encoder Units // 1 complete rotation = 4096 ticks
 
-        // START_MATCH is folded back
-        // GRAB_CUBE is at a ~45º angle, easier to drive up to a cube
-        // CLOSE_ARMS is where the arms are parallel [NOT NEEDED]
+        // FOLD_BACK is folded back
+        // OPEN_ARMS is at a ~45º angle, easier to drive up to a cube
+        // GRAB_CUBE is where the arms are parallel [NOT NEEDED]
 
         private int target;
 

src/main/java/org/whsrobotics/utils/AutomationCanceler.java

@@ -0,0 +1,15 @@
+package org.whsrobotics.utils;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class AutomationCanceler extends Command {
+
+    public AutomationCanceler() {
+        requires(new AutomationCancelerHelper());
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return true;
+    }
+}

src/main/java/org/whsrobotics/utils/AutomationCancelerHelper.java

@@ -0,0 +1,12 @@
+package org.whsrobotics.utils;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class AutomationCancelerHelper extends Subsystem {
+
+    @Override
+    protected void initDefaultCommand() {
+
+    }
+
+}