Java 类edu.wpi.first.wpilibj.command.Command 实例源码

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {


    chooser = new SendableChooser<Command>();
    chassis = new Chassis();
    intake = new Intake();
    winch = new Winch();
//  shooter = new Shooter();

    oi = new OI();
    imu = new ADIS16448_IMU(ADIS16448_IMU.Axis.kX);
    pdp = new PowerDistributionPanel();

    chooser.addDefault("Drive Straight", new DriveStraight(73, 0.5));
    //chooser.addObject("Left Gear Curve", new LeftGearCurve());
    chooser.addObject("Left Gear Turn", new LeftGearTurn());
    //chooser.addObject("Right Gear Curve", new RightGearCurve());
    chooser.addObject("Right Gear Turn", new RightGearTurn());
    chooser.addObject("Middle Gear", new StraightGear());
    chooser.addObject("Turn in place", new TurnDegrees(60, .2));
    SmartDashboard.putData("Autonomous Chooser", chooser);
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString code to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional commands to the
 * chooser code above (like the commented example) or additional comparisons
 * to the switch structure below with additional strings & commands.
 */
@Override
public void autonomousInit() {
    mAutonomousCommand = (Command) autoChooser.getSelected();
    //mAutonomousCommand.start();
    if (mAutonomousCommand != null)
        mAutonomousCommand.start();
    /*
     * String autoSelected = SmartDashboard.getString("Auto Selector",
     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
     * = new MyAutoCommand(); break; case "Default Auto": default:
     * autonomousCommand = new ExampleCommand(); break; }
     */

    /*
    // schedule the autonomous command (example)
    if (autonomousCommand != null)
        autonomousCommand.start();
    */
}

public void robotInit() {

    RobotMap.init();

    drivetrain = new Drivetrain();
    climber = new Climber();
    // fuel = new Fuel();
    gear = new Gear();

    oi = new OI();

    // initializes camera server
    server = CameraServer.getInstance();
    // cameraInit();
    // server.startAutomaticCapture(0);

    // autonomousCommand = (Command) new AutoMiddleHook();
    autonomousCommand = (Command) new AutoBaseline();

    // resets all sensors
    resetAllSensors();

    if (RobotConstants.isTestingEnvironment) updateTestingEnvironment();
    updateSensorDisplay();
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString code to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional commands to the
 * chooser code above (like the commented example) or additional comparisons
 * to the switch structure below with additional strings & commands.
 */
// @Override
public void autonomousInit() {

    // DEBUG \\
    if (RobotConstants.isTestingEnvironment) readTestingEnvironment();

    // resets sensors
    resetAllSensors();

    autonomousCommand = (Command) autoChooser.getSelected();

    // schedule the autonomous command (example)
    if (autonomousCommand != null)
        autonomousCommand.start();
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java CustomDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString line to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional comparisons to the
 * switch structure below with additional strings. If using the
 * SendableChooser make sure to add them to the chooser code above as well.
 */
@Override
public void autonomousInit() {
    driveTrain.resetEncoders();
    driveTrain.resetGyro();

    autoSelected = (Command) chooser.getSelected();
    if(autoSelected instanceof Initializer) {
        ((Initializer)autoSelected).init();
    }
    autoSelected.start();
    // autoSelected = CustomDashboard.getString("Auto Selector",
    // defaultAuto);
    System.out.println("Auto selected: " + autoSelected);
    pollPreferences();
}

public static ArrayList<Command> takeInput(String movement, boolean isRev, int shooting)
{
    auto_Commands = new ArrayList<Command>(0);
    if(movement.charAt(0) == 'f')
        auto_Commands.add(new MoveForward(Double.valueOf(movement.substring(1))));
    else if(movement.charAt(0) == 'r')
        auto_Commands.add(new RotateAngle(Double.valueOf(movement.substring(1))));
    else if(movement.charAt(0) == 's')
        auto_Commands.add(new SpyBot(shooting));
    //else if(movement.charAt(0) == 'l')
    //  auto_Commands.add(new CrossLowbar());

    if(isRev == true && movement.charAt(0) == 'f')
        auto_Commands.add(new MoveForward(-0.65*Double.valueOf(movement.substring(1)))); //This has the 0.65 because we don't want to accidentially cross the autoline when we go back


    return auto_Commands; //an arraylist of the commands to be executed in autonomous
}

public void teleopInit() {
    // This makes sure that the autonomous stops running when
    // teleop starts running. If you want the autonomous to
    // continue until interrupted by another command, remove
    // this line or comment it out.
    if (autonomousCommand != null) {
        autonomousCommand.cancel();
    }
    if (Robot.catapult.getPreviousShooterState() != 0) {
        Robot.catapult.setShooterState(Robot.catapult.getPreviousShooterState());
        Command cmd1 = new Shoot();
        cmd1.start();

    }
    Robot.robotDrive.setIsInAuto(false);
}

/**
 * Default constructor.
 *
 * @param buttons              The buttons for controlling this robot. Can be null for an empty list.
 * @param logger               The logger for recording events and telemetry data.
 * @param updater              A runnable that updates cached variables.
 * @param defaultCommands      The default commands for various subsystems.
 * @param autoStartupCommand   The command to be run when first enabled in autonomous mode.
 * @param teleopStartupCommand The command to be run when first enabled in teleoperated mode.
 * @param startupCommand       The command to be run when first enabled.
 */
@JsonCreator
public RobotMap(@Nullable List<CommandButton> buttons,
                @NotNull @JsonProperty(required = true) Logger logger,
                @NotNull @JsonProperty(required = true) MappedRunnable updater,
                @Nullable List<DefaultCommand> defaultCommands,
                @Nullable Command autoStartupCommand,
                @Nullable Command teleopStartupCommand,
                @Nullable Command startupCommand) {
    this.buttons = buttons != null ? buttons : new ArrayList<>();
    this.logger = logger;
    this.updater = updater;
    this.defaultCommands = defaultCommands;
    this.autoStartupCommand = autoStartupCommand;
    this.teleopStartupCommand = teleopStartupCommand;
    this.startupCommand = startupCommand;
}

/**
 * Default constructor.
 *
 * @param button  The button that triggers the command.
 * @param command The command to run or cancel.
 * @param action  The action to do to the command.
 */
@JsonCreator
public CommandButton(@NotNull @JsonProperty(required = true) MappedButton button,
                     @NotNull @JsonProperty(required = true) Command command,
                     @NotNull @JsonProperty(required = true) Action action) {
    switch (action) {
        case WHILE_HELD:
            button.whileHeld(command);
            break;
        case WHEN_PRESSED:
            button.whenPressed(command);
            break;
        case WHEN_RELEASED:
            button.whenReleased(command);
            break;
        case CANCEL_WHEN_PRESSED:
            button.cancelWhenPressed(command);
            break;
        case TOGGLE_WHEN_PRESSED:
            button.toggleWhenPressed(command);
            break;
    }
}

/**
 * Starts the given command whenever the trigger just becomes active.
 *
 * @param command the command to start
 */
public void whenActive(final Command command) {
  new ButtonScheduler() {

    private boolean m_pressedLast = grab();

    @Override
    public void execute() {
      if (grab()) {
        if (!m_pressedLast) {
          m_pressedLast = true;
          command.start();
        }
      } else {
        m_pressedLast = false;
      }
    }
  }.start();
}

/**
 * Constantly starts the given command while the button is held.
 *
 * {@link Command#start()} will be called repeatedly while the trigger is active, and will be
 * canceled when the trigger becomes inactive.
 *
 * @param command the command to start
 */
public void whileActive(final Command command) {
  new ButtonScheduler() {

    private boolean m_pressedLast = grab();

    @Override
    public void execute() {
      if (grab()) {
        m_pressedLast = true;
        command.start();
      } else {
        if (m_pressedLast) {
          m_pressedLast = false;
          command.cancel();
        }
      }
    }
  }.start();
}

/**
 * Starts the command when the trigger becomes inactive.
 *
 * @param command the command to start
 */
public void whenInactive(final Command command) {
  new ButtonScheduler() {

    private boolean m_pressedLast = grab();

    @Override
    public void execute() {
      if (grab()) {
        m_pressedLast = true;
      } else {
        if (m_pressedLast) {
          m_pressedLast = false;
          command.start();
        }
      }
    }
  }.start();
}

/**
 * Toggles a command when the trigger becomes active.
 *
 * @param command the command to toggle
 */
public void toggleWhenActive(final Command command) {
  new ButtonScheduler() {

    private boolean m_pressedLast = grab();

    @Override
    public void execute() {
      if (grab()) {
        if (!m_pressedLast) {
          m_pressedLast = true;
          if (command.isRunning()) {
            command.cancel();
          } else {
            command.start();
          }
        }
      } else {
        m_pressedLast = false;
      }
    }
  }.start();
}

/**
 * Cancels a command when the trigger becomes active.
 *
 * @param command the command to cancel
 */
public void cancelWhenActive(final Command command) {
  new ButtonScheduler() {

    private boolean m_pressedLast = grab();

    @Override
    public void execute() {
      if (grab()) {
        if (!m_pressedLast) {
          m_pressedLast = true;
          command.cancel();
        }
      } else {
        m_pressedLast = false;
      }
    }
  }.start();
}

private Command parseDriveToPegUsingVisionCommand(List<String> args)
{
    double timeout = 4;
    double deadband = 6;

    if (args.size() >= 2)
    {
        timeout = Double.parseDouble(args.get(1));
    }

    if (args.size() >= 3)
    {
        deadband = Double.parseDouble(args.get(2));
    }

    return new DriveToPegUsingVision(mSnobot.getVisionManager(), mSnobot.getSnobotActor(), deadband, timeout);
}

private Command createTrajectoryCommand(String aFile)
{
    String pathFile = Properties2017.sAUTON_PATH_DIRECTORY.getValue() + "/" + aFile.trim();
    TextFileDeserializer deserializer = new TextFileDeserializer();
    Path p = deserializer.deserializeFromFile(pathFile);

    Command output = null;
    if (p == null)
    {
        addError("Could not read path file " + pathFile);
    }
    else
    {
        output = new TrajectoryPathCommand(mSnobot.getDriveTrain(), mSnobot.getPositioner(), p);
    }

    return output;
}

private Command createTrajStartToHopper(List<String> args)
{
    StartingPositions startPosition = mPositionChooser.getSelected();
    if (startPosition == null)
    {
        addError("Invalid starting position");
        return null;
    }

    boolean doClose = true;
    if (args.size() > 1 && args.get(1).equals("Far"))
    {
        doClose = false;
    }

    return TrajectoryStartToHopperFactory.createCommand(mSnobot, startPosition, doClose);
}

private Command createTrajGearToHopper(List<String> args)
{
    StartingPositions startPosition = mPositionChooser.getSelected();
    if (startPosition == null)
    {
        addError("Invalid starting position");
        return null;
    }

    boolean doClose = true;
    if (args.size() > 1 && args.get(1).equals("Far"))
    {
        doClose = false;
    }

    return TrajectoryGearToHopperFactory.createCommand(mSnobot, startPosition, doClose);
}

private Command createTurnPathCommand(List<String> args)
{
    PathConfig dudePathConfig = new PathConfig(Double.parseDouble(args.get(1)), // Endpoint
            Double.parseDouble(args.get(2)), // Max Velocity
            Double.parseDouble(args.get(3)), // Max Acceleration
            sEXPECTED_DT);

    ISetpointIterator dudeSetpointIterator;

    // TODO create dynamic iterator, way to switch
    if (true)
    {
        dudeSetpointIterator = new StaticSetpointIterator(dudePathConfig);
    }

    return new DriveTurnPath(mSnobot.getDriveTrain(), mSnobot.getPositioner(), dudeSetpointIterator);
}

private Command createDrivePathCommand(List<String> args)
{
    PathConfig dudePathConfig = new PathConfig(Double.parseDouble(args.get(1)), // Endpoint
            Double.parseDouble(args.get(2)), // Max Velocity
            Double.parseDouble(args.get(3)), // Max Acceleration
            sEXPECTED_DT);

    ISetpointIterator dudeSetpointIterator;

    // TODO create dynamic iterator, way to switch
    if (true)
    {
        PathGenerator dudePathGenerator = new PathGenerator();
        List<PathSetpoint> dudeList = dudePathGenerator.generate(dudePathConfig);
        dudeSetpointIterator = new StaticSetpointIterator(dudeList);
    }

    return new DriveStraightPath(mSnobot.getDriveTrain(), mSnobot.getPositioner(), dudeSetpointIterator);
}

private Command createTrajectoryCommand(String aFile)
{
    String pathFile = Properties2016.sAUTON_PATH_DIRECTORY.getValue() + "/" + aFile.trim();
    TextFileDeserializer deserializer = new TextFileDeserializer();
    Path p = deserializer.deserializeFromFile(pathFile);

    Command output = null;
    if (p == null)
    {
        addError("Could not read path file " + pathFile);
    }
    else
    {
        output = new TrajectoryPathCommand(mSnobot.getDriveTrain(), mSnobot.getPositioner(), p);
    }

    return output;
}

private Command createFudgePosition(List<String> args)
{
    double newX;
    double newY;
    if (args.get(1).equals("Same"))
    {
        newX = mSnobot.getPositioner().getXPosition();
    }
    else
    {
        newX = Double.parseDouble(args.get(1));
    }

    if (args.get(2).equals("Same"))
    {
        newY = mSnobot.getPositioner().getYPosition();
    }
    else
    {
        newY = Double.parseDouble(args.get(2));
    }

    return new FudgeThePosition(mSnobot.getPositioner(), newX, newY);
}

/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
  public void autonomousInit() {
      autonomousCommand = (Command) chooser.getSelected();

/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
    autonomousCommand = new MyAutoCommand();
    break;
case "Default Auto":
default:
    autonomousCommand = new ExampleCommand();
    break;
} */

    // schedule the autonomous command (example)
      if (autonomousCommand != null) autonomousCommand.start();

      belowLowBar.start();
  }

/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
  public void autonomousInit() {
      autonomousCommand = (Command) chooser.getSelected();

/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
    autonomousCommand = new MyAutoCommand();
    break;
case "Default Auto":
default:
    autonomousCommand = new ExampleCommand();
    break;
} */

    // schedule the autonomous command (example)
      if (autonomousCommand != null) autonomousCommand.start();
  }

/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
  public void autonomousInit() {
      autonomousCommand = (Command) chooser.getSelected();

/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
    autonomousCommand = new MyAutoCommand();
    break;
case "Default Auto":
default:
    autonomousCommand = new ExampleCommand();
    break;
} */

    // schedule the autonomous command (example)
      if (autonomousCommand != null) autonomousCommand.start();
  }

/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
  public void autonomousInit() {
      autonomousCommand = (Command) chooser.getSelected();

/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
    autonomousCommand = new MyAutoCommand();
    break;
case "Default Auto":
default:
    autonomousCommand = new ExampleCommand();
    break;
} */

    // schedule the autonomous command (example)
      if (autonomousCommand != null) autonomousCommand.start();
  }

public  AimCleatShot(Direction direction) {
    Command turntableCommand = null;

    switch (direction) {
        case LEFT:
            turntableCommand = new MoveTurnTable(270);
            break;
        case RIGHT:
            turntableCommand = new MoveTurnTable(90);
            break;
        case CENTER:
            turntableCommand = new MoveTurnTable(180);
            break;
        default:
            // Oh no!
            break;
    }

    addSequential(turntableCommand);
}

/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
  public void autonomousInit() {
      autonomousCommand = (Command) chooser.getSelected();

/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
    autonomousCommand = new MyAutoCommand();
    break;
case "Default Auto":
default:
    autonomousCommand = new ExampleCommand();
    break;
} */

    // schedule the autonomous command (example)
      if (autonomousCommand != null) autonomousCommand.start();
  }

public AutonomousCommand(Command defense_command) {
    requires(Robot.chassis);
    addSequential(defense_command);
    addSequential(new AimAndShootCommand());
    // Add Commands here:
    // e.g. addSequential(new Command1());
    //      addSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use addParallel()
    // e.g. addParallel(new Command1());
    //      addSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
}

public void autonomousInit() {
    // Use the selected autonomous command

    autonomousCommand = (Command) oi.autonomousProgramChooser.getSelected();
    //double desiredDistrance = preferences.getDouble("DesiredDistance", 9.0);
    //autonomousCommand = new AutonomousCommandToteStrategy();
    //autonomousCommand = new StrafeCommand(3, 0.7);
    //double desiredDistance = preferences.getDouble("DesiredDistance", 9.0);
    //autonomousCommand = new AutonomousCommandToteStrategy();

    // Sets the setPoint to where-ever it is to prevent the elevator
    // wanting to go to a random position (default zero)
    elevator.setHieghtToCurrentPosition();
    // Tells the elevator to approximate the other maximum when it hits a limit switch
    Elevator.SAFETY = true;
    Elevator.needToApproximate = true;
    Elevator.didSaveTopValue = false;
    Elevator.didSaveBottomValue = false;
    driveTrain.fieldMode = false;
    autonomousCommand.start();
}

public void autonomousInit() {

    toteCollectTime = prefs.getDouble("toteTimeout", 1.0);
    driveToAutoTime = prefs.getDouble("timeToAutozone", 4.0);
    driveToAutoSpeed = prefs.getFloat("driveSpeedToScore", -0.8f);
    driveToBinTime = prefs.getDouble("timeToBin", 1.0);
    driveToBinSpeed = prefs.getFloat("driveToBinSpeed", -0.5f);
    elevateToteTime = prefs.getDouble("timeToElevateTote", 1.0);
    rollersEjectTime = prefs.getDouble("timeForRollersToEject", 1.0);
    timeToGroundLevel = prefs.getDouble("timeToGroundLevel", 1.0);
    turnToAutoTime = prefs.getDouble("timeToTurnToAutoZone", 1.0);
    turnToAutoSpeed = prefs.getFloat("speedToTurnToAutoZone", 0.5f);
    binLiftTime = prefs.getDouble("timeToLiftBin", 1.0);
    clawOpenOnElevMove = prefs.getBoolean("clawOpenALot", true);
    clawCloseOnElevMove = prefs.getBoolean("clawCloseALot", true);
    prefs.save();

    // schedule the autonomous command (example)
    autonomousCommand = (Command) chooser.getSelected();
    if (autonomousCommand != null) autonomousCommand.start();
}

public void autonomousInit() {
    // schedule the autonomous command
    //RobotMap.forkliftMotor.enableBrakeMode(true);  //TODO: verify that this is how you do it 

    /*
     * RobotMap.driveBaseLeftFrontMotor.enableBrakeMode(true);
     * RobotMap.driveBaseRightFrontMotor.enableBrakeMode(true);
    RobotMap.driveBaseLeftRearMotor.enableBrakeMode(true);
    RobotMap.driveBaseRightRearMotor.enableBrakeMode(true);
     */

    RobotMap.imu.zeroYaw();

    RobotMap.driveBaseLeftFrontMotor.enableBrakeMode(false);
    RobotMap.driveBaseRightFrontMotor.enableBrakeMode(false);
    RobotMap.driveBaseLeftRearMotor.enableBrakeMode(false);
    RobotMap.driveBaseRightRearMotor.enableBrakeMode(false);

    Scheduler.getInstance().add((Command) oi.pattern.getSelected());
}

/**
 * Starts the given command whenever the trigger just becomes active.
 * @param command the command to start
 */
public void whenActive(final Command command) {
    new ButtonScheduler() {

        boolean pressedLast = grab();

        public void execute() {
            if (grab()) {
                if (!pressedLast) {
                    pressedLast = true;
                    command.start();
                }
            } else {
                pressedLast = false;
            }
        }
    }.start();
}

/**
 * Constantly starts the given command while the button is held.
 *
 * {@link Command#start()} will be called repeatedly while the trigger is active,
 * and will be canceled when the trigger becomes inactive.
 *
 * @param command the command to start
 */
public void whileActive(final Command command) {
    new ButtonScheduler() {

        boolean pressedLast = grab();

        public void execute() {
            if (grab()) {
                pressedLast = true;
                command.start();
            } else {
                if (pressedLast) {
                    pressedLast = false;
                    command.cancel();
                }
            }
        }
    }.start();
}

/**
 * Starts the command when the trigger becomes inactive
 * @param command the command to start
 */
public void whenInactive(final Command command) {
    new ButtonScheduler() {

        boolean pressedLast = grab();

        public void execute() {
            if (grab()) {
                pressedLast = true;
            } else {
                if (pressedLast) {
                    pressedLast = false;
                    command.start();
                }
            }
        }
    }.start();
}

/**
 * Toggles a command when the trigger becomes active
 * @param command the command to toggle
 */
public void toggleWhenActive(final Command command) {
     new ButtonScheduler() {

        boolean pressedLast = grab();

        public void execute() {
            if (grab()) {
                if (!pressedLast) {
                    pressedLast = true;
                    if (command.isRunning()){
                        command.cancel();
                    } else{
                        command.start();
                    }
                }
            } else {
                pressedLast = false;
            }
        }
    }.start();
}

/**
 * Cancels a command when the trigger becomes active
 * @param command the command to cancel
 */
public void cancelWhenActive(final Command command) {
     new ButtonScheduler() {

        boolean pressedLast = grab();

        public void execute() {
            if (grab()) {
                if (!pressedLast) {
                    pressedLast = true;
                    command.cancel();
                }
            } else {
                pressedLast = false;
            }
        }
    }.start();
}

public void autonomousInit() {
//        // Last ditch effort to bring the camera up on the DS laptop, probably
//        // is too late.
//        TargetTrackingCommunication.setCameraEnabled(true);
//        // Tell the DS laptop to starting detecting the hot target
//        TargetTrackingCommunication.setAutonomousVisionRunning(true);
//        Removed this and put it in the autonomous command
//        Robot.driveSubsystem.getRobotDrive().resetGyro();

        // Pick which autonomous mode to use.
        Object selection = autonomousChooser.getSelected();
        if (selection != null && selection instanceof Command) {
            autonomousCommand = (Command) selection;
            autonomousCommand.start();
        } else {
            System.out.println("No autonomous mode selected.");
        }
    }

private Command _changeIntake(final int diff) {
    return new DoNothing() {
        protected void initialize() {
            _intake += diff;
            if(_intake >= 1) {
                Robot.intake.spinIn();
            } else if(_intake <= -1) {
                Robot.intake.spinOut();
            } else {
                Robot.intake.stop();
            }
        }
        protected boolean isFinished() {
            return true;
        }
    };
}

public Conditional(final Command ifTrue,final Command ifFalse) {
    super("Condition?" + (ifTrue  == null ? "" : ifTrue .getName()) +
                   ":" + (ifFalse == null ? "" : ifFalse.getName()));
    // Wrap the Commands to expose protected methods
    if(ifTrue != null) {
        _ifTrue  = new PublicCommand(ifTrue);
        for(Enumeration e = _ifTrue.getRequirements();e.hasMoreElements();) {
            requires((Subsystem) e.nextElement());
        }
    } else {
        _ifTrue = null;
    }
    if(ifFalse != null) {
        _ifFalse  = new PublicCommand(ifFalse);
        for(Enumeration e = _ifFalse.getRequirements();e.hasMoreElements();) {
            requires((Subsystem) e.nextElement());
        }
    } else {
        _ifFalse = null;
    }
}