Add compressor control code and fix solenoid ports

[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.driving.ToggleCompressor;

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;

  // 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;
  public static Button toggleCompressor;

  // 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(rightJoystick,
        Constants.OI.RIGHT_JOYSTICK_TRIGGER_PORT);
    toggleGear.toggleWhenPressed(new ChangeGear());

    toggleCompressor = new JoystickButton(rightJoystick,
        Constants.OI.TOGGLE_COMPRESSOR_PORT);
    toggleCompressor.whenPressed(new ToggleCompressor());

    // 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());
    // }
  }
}