Power Up Joystick Control Scheme

ControlElevator.java

package org.frc5587.robot2018.commands;

import org.frc5587.robot2018.OI;
import org.frc5587.robot2018.Robot;
import org.frc5587.robot2018.subsystems.Elevator;
import org.frc5587.robot2018.subsystems.Elevator.HeightLevels;

import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.Joystick;

public class ControlElevator extends Command {
    private boolean elevatorPistonsOn = false; 
    private Joystick jb;
    private Elevator elevator;

    public ControlElevator() {
        requires(Robot.elevator);
        elevator = Robot.elevator;
        jb = OI.joystick;
    }

    protected void initialize() {

    }

    protected void execute() {
       
        // Control elevator movement
        if (jb.getRawButton(4)) {
            elevator.goToHeight(HeightLevels.getPreviousValue(elevator.getHeightLevel()));
        } else if (jb.getRawButton(5)) {
            elevator.goToHeight(HeightLevels.getNextValue(elevator.getHeightLevel()));
        } else if (elevator.isDoneMoving()) {
            elevator.holdWithVoltage();
        }
            
        // Toggle position of the elevator pistons
        if (jb.getRawButton(8)) {
            if (elevatorPistonsOn) {
                elevator.triggerPistons(Value.kForward);
            } else {
                elevator.triggerPistons(Value.kReverse);
            }
            elevatorPistonsOn = !elevatorPistonsOn;
        }

        elevator.sendDebugInfo();
    }

    protected boolean isFinished() {
        return false;
    }

    protected void end() {
        elevator.stop();
    }

    protected void interrupted() {
        end();
    }
}

ControlGrabber.java

package org.frc5587.robot2018.commands;

import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.command.Command;

import org.frc5587.robot2018.OI;
import org.frc5587.robot2018.Robot;
import org.frc5587.robot2018.subsystems.Grabber;
import org.frc5587.robot2018.subsystems.Grabber.MotorSpeed;

public class ControlGrabber extends Command {
    MotorSpeed speed = MotorSpeed.OFF;
    Grabber grabber;
    Joystick jb;

    public ControlGrabber() {
        requires(Robot.grabber);
        grabber = Robot.grabber;
        jb = OI.joystick;
    }

    protected void initialize() {

    }

    protected void execute() {
        
        if (jb.getRawButton(3)) {
            speed = Grabber.MotorSpeed.EJECT;
        } else if (jb.getRawButton(2)) {
            speed = Grabber.MotorSpeed.INTAKE;
        } else {
            speed = Grabber.MotorSpeed.OFF;
        }

        grabber.setMotors(speed);

        // Piston bound to triggers (both do the same thing)
        if (jb.getRawButton(1)) {
            grabber.setPistons(DoubleSolenoid.Value.kReverse);
        } else {
            grabber.setPistons(DoubleSolenoid.Value.kForward);
        }
    }

    protected boolean isFinished() {
        return false;
    }

    protected void end() {
    }

    protected void interrupted() {
        end();
    }
}