- 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(/* TO DO: define this, and fill in commands */);
-
- 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());
-
- intakeBoulder.whenPressed(new IntakeBall());
- shootBoulder.whenPressed(new Shoot());
-
- } else if (isScalingMode) {
- toggleShooter.toggleWhenPressed(new WinchIn());
- compactRobot_1.whenPressed(new RetractLift());
-
- intakeBoulder.toggleWhenPressed(new WinchOut());
- shootBoulder.whenPressed(new ExtendLift());
- }
-
- SpinRobot180_1
- .whenPressed(/* rotate robot 180, reorient joystick controls */);
-
+ // Left joystick
+ gearShift = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_GEAR_SHIFT_PORT);
+ gearShift.whenPressed(new SetLowGear());
+ gearShift.whenReleased(new SetHighGear());
+
+ extendIntake1 = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_EXTEND_INTAKE_1_PORT);
+ extendIntake1.whenPressed(new MoveIntakeArm(Constants.IntakeArm.EXTEND));
+
+ extendIntake2 = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_EXTEND_INTAKE_2_PORT);
+ extendIntake2.whenPressed(new MoveIntakeArm(Constants.IntakeArm.EXTEND));
+
+ retractIntake1 = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_RETRACT_INTAKE_1_PORT);
+ retractIntake1.whenPressed(new MoveIntakeArm(Constants.IntakeArm.RETRACT));
+
+ retractIntake2 = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_RETRACT_INTAKE_2_PORT);
+ retractIntake2.whenPressed(new MoveIntakeArm(Constants.IntakeArm.RETRACT));
+
+ toggleFront = new JoystickButton(leftJoystick,
+ Constants.OI.LEFT_JOYSTICK_TOGGLE_FRONT_PORT);
+ toggleFront.whenPressed(new ToggleFront());
+
+ // Right joystick
+ intake = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_INTAKE_PORT);
+ intake.whenPressed(new RunIntake(Constants.IntakeArm.IN));
+ intake.whenReleased(new RunIntake(Constants.IntakeArm.STOP));
+
+ outtake1 = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_OUTTAKE_1_PORT);
+ outtake1.whenPressed(new RunIntake(Constants.IntakeArm.OUT));
+ outtake1.whenReleased(new RunIntake(Constants.IntakeArm.STOP));
+
+ outtake2 = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_OUTTAKE_2_PORT);
+ outtake2.whenPressed(new RunIntake(Constants.IntakeArm.OUT));
+ outtake2.whenReleased(new RunIntake(Constants.IntakeArm.STOP));
+
+ shooterUp = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_SHOOTER_UP_PORT);
+ shooterUp.whenPressed(new FireCatapult());
+
+ shooterDown1 = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_SHOOTER_DOWN_1_PORT);
+ shooterDown1.whenPressed(new ResetCatapult());
+
+ shooterDown2 = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_SHOOTER_DOWN_2_PORT);
+ shooterDown2.whenPressed(new ResetCatapult());