add commands to digital buttons in OI and create/edit necessary commands

[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.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;
import edu.wpi.first.wpilibj.command.Scheduler;

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 toggleScalingMode;

  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));
    passChevalDeFrise = new DigitalButton(
        new DigitalInput(Constants.OI.PASS_CHEVAL_DE_FRISE_PORT));
    passDrawbridge = new DigitalButton(
        new DigitalInput(Constants.OI.PASS_DRAWBRIDGE));
    passSallyPort = new DigitalButton(
        new DigitalInput(Constants.OI.PASS_SALLYPORT_PORT));

    lowerChevalDeFrise = new DigitalButton(
        new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_ONE_PORT));
    moveToIntakeBoulder = new DigitalButton(
        new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_TWO_PORT));
    poiseAboveChevalDeFrise = new DigitalButton(
        new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_THREE_PORT));
    moveIntakeArmInsideRobot = new DigitalButton(
        new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_FOUR_PORT));

    toggleShooter = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TRIGGER_PORT);
    SpinRobot180_1 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_LEFT_PORT);
    SpinRobot180_2 = new JoystickButton(leftJoystick,
        Constants.OI.LEFT_JOYSTICK_TOP_RIGHT_PORT);
    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);

    toggleScalingMode = new DigitalButton(
        new DigitalInput(Constants.OI.SCALING_BUTTON_PORT));

    passPortcullis.whenPressed(new PassPortcullis());
    passChevalDeFrise.whenPressed(new PassChevalDeFrise());
    passDrawbridge.whenPressed(new PassDrawBridge());
    passSallyPort.whenPressed(new PassSallyPort());

    lowerChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
        IntakeArm.potAngles[0], IntakeArm.moveIntakeArmSpeed));
    moveToIntakeBoulder.whenPressed(new MoveIntakeArmToAngle(
        IntakeArm.potAngles[1], IntakeArm.moveIntakeArmSpeed));
    poiseAboveChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
        IntakeArm.potAngles[2], IntakeArm.moveIntakeArmSpeed));
    moveIntakeArmInsideRobot.whenPressed(new MoveIntakeArmToAngle(
        IntakeArm.potAngles[3], IntakeArm.moveIntakeArmSpeed));

    if (toggleScalingMode.get()) {
      if (!isScalingMode) {
        isScalingMode = true;
        if (!isCompactRobot) {
          Scheduler.getInstance().add(new CompactRobot());
          isCompactRobot = true;
        }
      } else if (isScalingMode) {
        isScalingMode = false;
        Scheduler.getInstance().add(new CompactRobot());
      }
    }

    if (!isScalingMode) {
      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.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());
    }

    SpinRobot180_1.whenPressed(new Turn180());
    SpinRobot180_2.whenPressed(new Turn180());
  }
}