Fix formatting and stuff

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

package org.usfirst.frc.team3501.robot;

import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot;
import org.usfirst.frc.team3501.robot.commands.auton.PassChevalDeFrise;
import org.usfirst.frc.team3501.robot.commands.auton.PassDrawBridge;
import org.usfirst.frc.team3501.robot.commands.auton.PassPortcullis;
import org.usfirst.frc.team3501.robot.commands.auton.PassSallyPort;
import org.usfirst.frc.team3501.robot.commands.driving.Turn180;
import org.usfirst.frc.team3501.robot.commands.intakearm.IntakeBall;
import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArmToAngle;
import org.usfirst.frc.team3501.robot.commands.scaler.ExtendLift;
import org.usfirst.frc.team3501.robot.commands.scaler.RetractLift;
import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous;
import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch;
import org.usfirst.frc.team3501.robot.commands.scaler.ToggleScaling;
import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
import org.usfirst.frc.team3501.robot.commands.shooter.runShooter;
import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;

import edu.wpi.first.wpilibj.DigitalInput;
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 boolean isScalingMode = false;
  public static boolean isCompactRobot = false;

  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;

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

    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 if (isScalingMode) {
      // 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.WINCH_IN_SPEED));
      compactRobot_1.whenPressed(new RetractLift());
      compactRobot_2.whenPressed(new RetractLift());

      intakeBoulder.whenReleased(new StopWinch());
      shootBoulder.whenPressed(new ExtendLift());
    }
  }
}