[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / OI.java

package org.usfirst.frc.team3501.robot;

import org.usfirst.frc.team3501.robot.commands.driving.ChangeGear;
import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
import org.usfirst.frc.team3501.robot.commands.intakearm.RunBothIntakeMotors;
import org.usfirst.frc.team3501.robot.commands.intakearm.RunIntakeMotor;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;

public class OI {
  public static Joystick leftJoystick;
  public static Joystick rightJoystick;

  public static Button leftIntakeArmMotorUp;
  public static Button leftIntakeArmMotorDown;
  public static Button rightIntakeArmMotorUp;
  public static Button rightIntakeArmMotorDown;
  public static Button bothIntakeArmMotorUp;
  public static Button bothIntakeArmMotorDown;

  // // first column of arcade buttons - getting past defenses
  // public static DigitalButton passPortcullis;
  // public static DigitalButton passChevalDeFrise;
  // public static DigitalButton passDrawbridge;
  // public static DigitalButton passSallyPort;
  //
  // // second column of arcade buttons - different angles for intake arm
  // // TO DO: change position numbers to angle values (?)
  // public static DigitalButton lowerChevalDeFrise;
  // public static DigitalButton moveToIntakeBoulder;
  // public static DigitalButton poiseAboveChevalDeFrise;
  // public static DigitalButton moveIntakeArmInsideRobot;
  //
  // // left joystick buttons
  // public static Button toggleShooter;
  // public static Button SpinRobot180_1; // both do the same thing, just two
  // public static Button SpinRobot180_2; // different buttons
  // public static Button compactRobot_1;
  // public static Button compactRobot_2;
  //
  // right joystick buttons
  public static Button intakeBoulder;
  public static Button shootBoulder;
  public static Button toggleGear;
  //
  // // button to change robot to the scaling mode
  // public static DigitalButton toggleScaling;

  public OI() {
    leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
    rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);

    toggleGear = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_CENTER_PORT);
    toggleGear.toggleWhenPressed(new ChangeGear());

    shootBoulder = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TRIGGER_PORT);
    shootBoulder.whenPressed(new Shoot());

    recordPotAngle1 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_LEFT_BUTTON);
    recordPotAngle1.whenPressed(new RecordPotAngle(0));
    recordPotAngle2 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_CENTER_BUTTON);
    recordPotAngle2.whenPressed(new RecordPotAngle(1));
    recordPotAngle3 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_RIGHT_BUTTON);
    recordPotAngle3.whenPressed(new RecordPotAngle(2));
    recordPotAngle4 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_LOW_BUTTON);
    recordPotAngle4.whenPressed(new RecordPotAngle(3));

    leftIntakeArmMotorUp = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_LEFT_FORWARD_BUTTON);
    leftIntakeArmMotorUp.whenPressed(new RunIntakeMotor(
        Robot.intakeArm.getLeftIntakeArmMotor(),
        Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));
    leftIntakeArmMotorDown = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_LEFT_BACK_BUTTON);
    leftIntakeArmMotorDown.whenPressed(new RunIntakeMotor(
        Robot.intakeArm.getLeftIntakeArmMotor(),
        -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));

    rightIntakeArmMotorUp = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_RIGHT_FORWARD_BUTTON);
    rightIntakeArmMotorUp.whenPressed(new RunIntakeMotor(
        Robot.intakeArm.getRightIntakeArmMotor(),
        Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));
    rightIntakeArmMotorDown = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_RIGHT_BACK_BUTTON);
    rightIntakeArmMotorDown.whenPressed(new RunIntakeMotor(
        Robot.intakeArm.getRightIntakeArmMotor(),
        -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));

    bothIntakeArmMotorUp = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_BACK_LEFT_BUTTON);
    bothIntakeArmMotorUp.whenPressed(new RunBothIntakeMotors(
        Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));
    bothIntakeArmMotorDown = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_BOTTOM_BACK_RIGHT_BUTTON);
    bothIntakeArmMotorDown.whenPressed(new RunBothIntakeMotors(
        -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED));

    // passPortcullis = new DigitalButton(
    // new DigitalInput(Constants.OI.PASS_PORTCULLIS_PORT));
    // passPortcullis.whenPressed(new PassPortcullis());
    //
    // passChevalDeFrise = new DigitalButton(
    // new DigitalInput(Constants.OI.PASS_CHEVAL_DE_FRISE_PORT));
    // passChevalDeFrise.whenPressed(new PassChevalDeFrise());
    //
    // passDrawbridge = new DigitalButton(
    // new DigitalInput(Constants.OI.PASS_DRAWBRIDGE_PORT));
    // passDrawbridge.whenPressed(new PassDrawBridge());
    //
    // passSallyPort = new DigitalButton(
    // new DigitalInput(Constants.OI.PASS_SALLYPORT_PORT));
    // passSallyPort.whenPressed(new PassSallyPort());
    //
    // lowerChevalDeFrise = new DigitalButton(
    // new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_ONE_PORT));
    // lowerChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
    // IntakeArm.potAngles[0], IntakeArm.moveIntakeArmSpeed));
    //
    // moveToIntakeBoulder = new DigitalButton(
    // new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_TWO_PORT));
    // moveToIntakeBoulder.whenPressed(new MoveIntakeArmToAngle(
    // IntakeArm.potAngles[1], IntakeArm.moveIntakeArmSpeed));
    //
    // poiseAboveChevalDeFrise = new DigitalButton(
    // new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_THREE_PORT));
    // poiseAboveChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
    // IntakeArm.potAngles[2], IntakeArm.moveIntakeArmSpeed));
    //
    // moveIntakeArmInsideRobot = new DigitalButton(
    // new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_FOUR_PORT));
    // moveIntakeArmInsideRobot.whenPressed(new MoveIntakeArmToAngle(
    // IntakeArm.potAngles[3], IntakeArm.moveIntakeArmSpeed));
    //
    // toggleShooter = new JoystickButton(leftJoystick,
    // Constants.OI.LEFT_JOYSTICK_TRIGGER_PORT);
    // SpinRobot180_1 = new JoystickButton(leftJoystick,
    // Constants.OI.SPIN1_PORT);
    // SpinRobot180_1.whenPressed(new Turn180());
    //
    // SpinRobot180_2 = new JoystickButton(leftJoystick,
    // Constants.OI.SPIN2_PORT);
    // SpinRobot180_2.whenPressed(new Turn180());
    //
    // compactRobot_1 = new JoystickButton(leftJoystick,
    // Constants.OI.LEFT_JOYSTICK_TOP_CENTER_PORT);
    // compactRobot_2 = new JoystickButton(leftJoystick,
    // Constants.OI.LEFT_JOYSTICK_TOP_LOW_PORT);
    //
    // intakeBoulder = new JoystickButton(rightJoystick,
    // Constants.OI.RIGHT_JOYSTICK_TRIGGER_PORT);
    // shootBoulder = new JoystickButton(rightJoystick,
    // Constants.OI.RIGHT_JOYSTICK_THUMB_PORT);
    //
    // toggleScaling = new DigitalButton(
    // new DigitalInput(Constants.OI.TOGGLE_SCALING_PORT));
    // toggleScaling.whenPressed(new ToggleScaling());
    //
    // if (!Constants.Scaler.SCALING) {
    // toggleShooter.toggleWhenPressed(new runShooter());
    // compactRobot_1.whenPressed(new CompactRobot());
    // compactRobot_2.whenPressed(new CompactRobot());
    //
    // intakeBoulder.whenPressed(new IntakeBall());
    // shootBoulder.whenPressed(new Shoot());
    //
    // } else {
    // // toggleShooter becomes winch
    // // compact robot button 1 and 2 retracts the lift
    // // intake button stops the winch
    // // shoot button extends the lift
    // toggleShooter.whenPressed(new RunWinchContinuous(
    // Constants.Scaler.SCALE_SPEED, Constants.Scaler.SECONDS_TO_SCALE));
    // compactRobot_1.whenPressed(new RetractLift());
    // compactRobot_2.whenPressed(new RetractLift());
    //
    // intakeBoulder.whenReleased(new StopWinch());
    // shootBoulder.whenPressed(new ExtendLift());
    // }
  }
}