コード例 #1
0
  public OI() {
    // Controllers
    ps4Controller = new Joystick(PS4_PORT);
    operatorControl = new Joystick(OPERATOR_PORT);
    manualOperatorControl = new Joystick(MANUAL_OPERATOR_PORT);

    // Manual Buttons
    JoystickButton openDocOcButton = new JoystickButton(manualOperatorControl, DOC_OC_OPEN);
    JoystickButton closeDocOcButton = new JoystickButton(manualOperatorControl, DOC_OC_CLOSE);
    JoystickButton openBearHugger = new JoystickButton(manualOperatorControl, CLOSE_BEAR_HUGGER);
    JoystickButton closeBearHugger = new JoystickButton(manualOperatorControl, OPEN_BEAR_HUGGER);
    JoystickButton bearHuggerUp = new JoystickButton(manualOperatorControl, BEAR_HUGGER_UP);
    JoystickButton bearHuggerDown = new JoystickButton(manualOperatorControl, BEAR_HUGGER_DOWN);
    JoystickButton homeBearHugger = new JoystickButton(manualOperatorControl, 1);

    // Manual Actions
    openDocOcButton.whenPressed(new ActuateDocOcGripper(ClawPosition.OPEN));
    closeDocOcButton.whenPressed(new ActuateDocOcGripper(ClawPosition.CLOSE));
    openBearHugger.whenPressed(new BearHuggerGripperCommand(ClawPosition.OPEN));
    closeBearHugger.whenPressed(new BearHuggerGripperCommand(ClawPosition.CLOSE));

    homeBearHugger.whenPressed(new HomeBearHugger());

    bearHuggerUp.whileHeld(new SimpleBearHuggerElevatorCommand(0.8));
    //		bearHuggerUp.whenReleased(new SimpleBearHuggerElevatorCommand(0));
    bearHuggerDown.whileHeld(new SimpleBearHuggerElevatorCommand(-0.75));
    //		bearHuggerDown.whenReleased(new SimpleBearHuggerElevatorCommand(0));

    // Operator Buttons
    JoystickButton groundButton = new JoystickButton(operatorControl, GROUND_BUTTON);
    JoystickButton carryPlaceButton = new JoystickButton(operatorControl, CARRY_PLACE_BUTTON);
    JoystickButton stepButton = new JoystickButton(operatorControl, STEP_BUTTON);
    JoystickButton oneToteButton = new JoystickButton(operatorControl, ONE_TOTE_BUTTON);
    JoystickButton twoToteButton = new JoystickButton(operatorControl, TWO_TOTE_BUTTON);
    JoystickButton humanPlayerButton = new JoystickButton(operatorControl, HUMAN_PLAYER_BUTTON);
    JoystickButton landfillButton = new JoystickButton(operatorControl, LANDFILL_BUTTON);
    // New Stuff
    JoystickButton tiltButton = new JoystickButton(operatorControl, TILT_BUTTON);
    JoystickButton pickUpButton = new JoystickButton(operatorControl, PICK_UP_BUTTON);
    JoystickButton bearHuggerButton = new JoystickButton(operatorControl, BEAR_OPEN_CLOSE);
    JoystickButton docOcButton = new JoystickButton(operatorControl, DOC_OC_SEQUENCE_BUTTON);

    // OP Button Actions
    tiltButton.whileHeld(new CanTiltCommand(TiltPosition.TILT));
    tiltButton.whenReleased(new CanTiltCommand(TiltPosition.UNTILT));

    bearHuggerButton.whileHeld(new BearHuggerGripperCommand(ClawPosition.OPEN));
    bearHuggerButton.whenReleased(new BearHuggerGripperCommand(ClawPosition.CLOSE));

    //		pickUpButton.whileHeld(new RollerGripperCommand(RollerGripperDirection.IN,
    // ClawPosition.CLOSE));
    //		pickUpButton.whenReleased(new RollerGripperCommand(RollerGripperDirection.OFF,
    // ClawPosition.OPEN));
    pickUpButton.whenPressed(new FromFloorToBearHuggerAndClear());

    docOcButton.whenPressed(new StowToBearHuggerAndClear());
    // End New Stuff

    groundButton.whenPressed(new DefaultGround());

    carryPlaceButton.whenPressed(new ElevateToteCommand(ElevatorPosition.CARRY));
    //		carryPlaceButton.whenReleased(new DefaultGround());

    stepButton.whenPressed(new ElevateToteCommand(ElevatorPosition.STEP_CARRY));
    //		stepButton.whenReleased(new ElevateToteCommand(ElevatorPosition.STEP_PLACE));

    oneToteButton.whenPressed(new ElevateToteCommand(ElevatorPosition.ONE_TOTE_CARRY));
    //		oneToteButton.whenPressed(new ElevateToteCommand(ElevatorPosition.ONE_TOTE_PLACE));

    twoToteButton.whenPressed(new ElevateToteCommand(ElevatorPosition.TWO_TOTE_CARRY));
    //		twoToteButton.whenReleased(new ElevateToteCommand(ElevatorPosition.TWO_TOTE_PLACE));

    humanPlayerButton.whenPressed(new UpAndOpen());
    humanPlayerButton.whenReleased(new AutomatedHPThing());

    landfillButton.whenPressed(new LandfillGrabbing());

    // Joystick Buttons
    //		JoystickButton shiftDownButton = new JoystickButton(ps4Controller, SHIFT_DOWN_BUTTON);
    JoystickButton shiftUpButton = new JoystickButton(ps4Controller, SHIFT_UP_BUTTON);
    JoystickButton scoreButton = new JoystickButton(ps4Controller, SCORE_BUTTON);
    JoystickButton carryButton = new JoystickButton(ps4Controller, CARRY_BUTTON);
    JoystickButton intakeButton = new JoystickButton(ps4Controller, INTAKE_BUTTON);
    JoystickButton ejectButton = new JoystickButton(ps4Controller, EJECT_BUTTON);
    JoystickButton groundWithoutOpenButton = new JoystickButton(ps4Controller, GROUND_WITHOUT_OPEN);
    JoystickButton resetBearHugger = new JoystickButton(ps4Controller, BEAR_RESET_BUTTON);
    JoystickButton justIntake = new JoystickButton(ps4Controller, JUST_INTAKE_BUTTON);

    // JS Button Actions
    shiftUpButton.whileHeld(new ShiftCommand(HIGH));
    shiftUpButton.whenReleased(new ShiftCommand(LOW));
    //		shiftDownButton.whenPressed(new ShiftCommand(LOW));
    scoreButton.whenPressed(new PlaceToteStackCommandGroup());
    carryButton.whenPressed(new ElevateToteCommand(ElevatorPosition.CARRY));
    ejectButton.whileHeld(new RollerGripperCommand(RollerGripperDirection.OUT, ClawPosition.CLOSE));
    intakeButton.whileHeld(new LandfillGrabbing());
    ejectButton.whenReleased(
        new RollerGripperCommand(RollerGripperDirection.OFF, ClawPosition.OPEN));
    intakeButton.whenReleased(
        new RollerGripperCommand(RollerGripperDirection.OFF, ClawPosition.OPEN));
    groundWithoutOpenButton.whenPressed(new FinishStack());
    resetBearHugger.whenPressed(new HomeBearHugger());
    justIntake.whileHeld(new RollerGripperCommand(RollerGripperDirection.IN, ClawPosition.CLOSE));
    justIntake.whenReleased(
        new RollerGripperCommand(RollerGripperDirection.OFF, ClawPosition.OPEN));

    //		throttleScalingButton.whenActive(new JoystickDrive());

    // Autonomous
    JoystickButton autoSelector1 = new JoystickButton(operatorControl, 1);
    JoystickButton autoSelector2 = new JoystickButton(operatorControl, 2);
    JoystickButton autoSelector3 = new JoystickButton(operatorControl, 3);

    autoSelector = new AutonomousSelector(autoSelector1, autoSelector2, autoSelector3);

    List<Command> autoCommands = new ArrayList<>();
    autoCommands.add(new TotesAndCansCommand());
    autoCommands.add(new DriveAtSpeed());

    autoSelector.setCommands(autoCommands);

    // Elevator commands
    //		SmartDashboard.putData("DriveToToteAtSpeed", new DriveAtSpeedToTote(48, 0.4, 60));
    //		SmartDashboard.putData(new TotesAndCansCommand());
    //		SmartDashboard.putData("Doc Oc Arm Up", new SimpleDocOcArmElevationCommand(0.3));
    //		SmartDashboard.putData("Doc Oc Arm Down", new SimpleDocOcArmElevationCommand(-0.1));
    //		SmartDashboard.putData("Doc Oc Arm Left", new SimpleDocOcArmYawCommand(0.1));
    //		SmartDashboard.putData("Doc Oc Arm Right", new SimpleDocOcArmYawCommand(-0.1));
    SmartDashboard.putData(
        "Bear Hugger Elevator Up",
        new SimpleBearHuggerElevatorCommand(BearHuggerElevator.BEAR_HUGGER_ELEVATOR_POWER));
    SmartDashboard.putData("Bear Hugger Elevator Down", new SimpleBearHuggerElevatorCommand(-0.75));
    SmartDashboard.putData("Doc Oc Arm Open", new ActuateDocOcGripper(ClawPosition.OPEN));
    SmartDashboard.putData("Doc Oc Arm Close", new ActuateDocOcGripper(ClawPosition.CLOSE));
    //		SmartDashboard.putData("Elevate to can", new
    // ElevateBearHuggerCommand(BearHuggerElevator.CAN_HAND_OFF, 0.75));
    SmartDashboard.putData("home tote", new HomeToteElevator());
    SmartDashboard.putData("Open Bear Hugger", new BearHuggerGripperCommand(ClawPosition.OPEN));
    SmartDashboard.putData("Close Bear Hugger", new BearHuggerGripperCommand(ClawPosition.CLOSE));
    //		SmartDashboard.putData("Arm To Floor", new
    // ElevateDocOcArmToCommand(DocOcArmPosition.LEFT_FLOOR, 0.5));
    //		SmartDashboard.putData("Arm To Step", new ElevateDocOcArmToCommand(Robot.leftDocOcArm,
    // DocOcArmPosition.LEFT_STEP, 12));
    //		SmartDashboard.putData("Arm To Stow", new
    // ElevateDocOcArmToCommand(DocOcArmPosition.LEFT_STOWED, 0.5));
    //		SmartDashboard.putData("Rotate Arm To Stow", new DocOcArmYawCommand(0.1,
    // DocOcArmPosition.LEFT_SECOND_CAN));
    //		SmartDashboard.putData("Rotate Arm To Floor", new DocOcArmYawCommand(0.1,
    // DocOcArmPosition.LEFT_FLOOR));
    SmartDashboard.putData(new FromFloorToBearHugger());
    SmartDashboard.putData("Tilt Can", new CanTiltCommand(TiltPosition.TILT));
    SmartDashboard.putData("Un-Tilt Can", new CanTiltCommand(TiltPosition.UNTILT));
    SmartDashboard.putData(new ElevateDocOcToYaw(1));
    SmartDashboard.putData(new TurnToAngle(90, 0.75));
    SmartDashboard.putData("Stow To Bear", new StowToBearHugger());
    SmartDashboard.putData("Drive Fo Eva", new DriveStraightAtSpeed(36, 81));
  }
コード例 #2
0
 public Command getAutoCommand() {
   return autoSelector.getSelectedCommand();
 }