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

/**
 * Parses a parallel command (commands separated by '|'
 * 
 * @param args
 *            The list of arguments
 * @return The command group for the parallel command
 */
protected CommandGroup parseParallelCommand(List<String> args)
{
    String parallel_line = "";
    for (int i = 1; i < args.size(); ++i)
    {
        parallel_line += args.get(i) + " ";
    }

    String[] split_commands = parallel_line.split("\\|");
    CommandGroup parallelCommands = new CommandGroup();

    for (String this_line : split_commands)
    {
        parseLine(parallelCommands, this_line, true);
    }

    return parallelCommands;
}

/**
 * Calculates distance to travel and proper orientation then creates
 * DriveStraightADistance and TurnWithDegrees Commands, adds them to a
 * CommandGroup, then starts the CommandGroup.
 */
@Override
protected void initialize()
{

    mCommandGroup = new CommandGroup();
    mCurrentX = mPositioner.getXPosition();
    mCurrentY = mPositioner.getYPosition();
    mDriveDistance = Math.sqrt((Math.pow((mFinalXCoor - mCurrentX), 2)) + (Math.pow((mFinalYCoor - mCurrentY), 2)));
    mTurnDegrees = Math.toDegrees(Math.atan2((mFinalXCoor - mCurrentX), (mFinalYCoor - mCurrentY)));
    mTurnWithDegrees = new TurnWithDegrees(mDriveTrain, mPositioner, mTurnDegrees, mSpeed);
    System.out.println(mTurnDegrees);
    mDriveStraightADistance = new DriveStraightADistance(mDriveTrain, mPositioner, mDriveDistance, mSpeed);
    mCommandGroup.addSequential(mTurnWithDegrees);
    mCommandGroup.addSequential(mDriveStraightADistance);
    mCommandGroup.start();

}

public CommandGroup buildAnAuton()
{
    CommandGroup cobbledCommandGroup = new CommandGroup();

    try
    {
        mSelectStartPosition.setStartPosition();

        mDefenseCommandParser.readFile(mDefenseInFront.getDefensePath()); // Forces a re-read, publish to dashboard

        cobbledCommandGroup.addSequential(mPostDefenseCommandParser.readFile(mSelectAutonomous.getSelected()));
    }
    catch (Exception e)
    {
        System.err.println("Could not read auton files");
        e.printStackTrace();
    }
    return cobbledCommandGroup;
}

/**
 * Init- if statement to decide which low goal to go to; also adds the
 * correct GoToXYPath command
 */
@Override
protected void initialize()
{
    double mYPosition = Properties2016.sLOW_GOAL_Y.getValue();
    double mXPosition = Properties2016.sLOW_GOAL_X.getValue();
    if (mPositioner.getXPosition() < 0)
    {
        mXPosition = -mXPosition;
    }
    GoToXYPath drive_close_to_goal = new GoToXYPath(mDriveTrain, mPositioner, (mXPosition + 15), (mYPosition - 30), mMaxTurnVel, mMaxTurnAccel,
            mMaxDriveVel,
            mMaxDriveAccel);
    GoToXYPath drive_to_goal = new GoToXYPath(mDriveTrain, mPositioner, mXPosition, mYPosition, mMaxTurnVel, mMaxTurnAccel, mMaxDriveVel, mMaxDriveAccel);

    mCommandGroup = new CommandGroup();
    mCommandGroup.addSequential(drive_close_to_goal);
    mCommandGroup.addSequential(drive_to_goal);
}

/**
 * Init- calculates the drive distance and turn degrees, plugs them into a
 * path command, and adds it to the command group.
 */
@Override
protected void initialize()
{
    mCurrentX = mPositioner.getXPosition();
    double ChangeInX = mFinalXCoor - mCurrentX;

    mCurrentY = mPositioner.getYPosition();
    double ChangeInY = mFinalYCoor - mCurrentY;

    mDriveDistance = Math.sqrt((Math.pow((ChangeInX), 2)) + (Math.pow((ChangeInY), 2)));
    mTurnDegrees = Math.toDegrees(Math.atan2((ChangeInX), (ChangeInY)));

    mTurnPathConfig = new PathConfig(mTurnDegrees, mMaxTurnVel, mMaxTurnAccel, .02);
    mTurnSetpointIterator = new StaticSetpointIterator(mTurnPathConfig);
    mDriveTurnPath = new DriveTurnPath(mDriveTrain, mPositioner, mTurnSetpointIterator);

    mDrivePathConfig = new PathConfig(mDriveDistance, mMaxDriveVel, mMaxDriveAccel, .02);
    mDriveSetpointIterator = new StaticSetpointIterator(mDrivePathConfig);
    mDriveStraightPath = new DriveStraightPath(mDriveTrain, mPositioner, mDriveSetpointIterator);

    mCommandGroup = new CommandGroup();
    mCommandGroup.addSequential(mDriveTurnPath);
    mCommandGroup.addSequential(mDriveStraightPath);
}

/**
 * Parses a parallel command (commands separated by '|'
 * 
 * @param args
 *            The list of arguments
 * @return The command group for the parallel command
 */
protected CommandGroup parseParallelCommand(List<String> args)
{
    String parallel_line = "";
    for (int i = 1; i < args.size(); ++i)
    {
        parallel_line += args.get(i) + " ";
    }

    String[] split_commands = parallel_line.split("\\|");
    CommandGroup parallelCommands = new CommandGroup();

    for (String this_line : split_commands)
    {
        parseLine(parallelCommands, this_line, true);
    }

    return parallelCommands;
}

private static CommandGroup driveAndPrepareToShoot(boolean checkHot, double driveDistance, double timeToFinish, double timeout) {
        CommandGroup driveAndCheckGoal = new CommandGroup("driveAndCheck");
        driveAndCheckGoal.addSequential(driveForwardRotate(0, 0));
        driveAndCheckGoal.addParallel(setFingerPosition(ClawFingerSubsystem.DOWN));
        driveAndCheckGoal.addParallel(new SetClawWinchSolenoid(true));
        CommandGroup setClawPosition = new CommandGroup();
//        check the hot goal after .5 seconds
        if (checkHot) {
            driveAndCheckGoal.addSequential(new WaitCommand(500));
            driveAndCheckGoal.addSequential(new HotVisionWaitCommand());
            timeToFinish += 4.8;
        }
        final double minDriveSpeed = .7;

        ChangeableBoolean driveFinishedChecker = new ChangeableBoolean(false);
        driveAndCheckGoal.addParallel(new DriveSetDistanceWithPIDCommand(driveDistance, minDriveSpeed, driveFinishedChecker));
        driveAndCheckGoal.addSequential(new SetClawPosition(ClawPivotSubsystem.BACKWARD_SHOOT, driveFinishedChecker, timeToFinish), timeout);
        return driveAndCheckGoal;
    }

public void autonomousInit() {

    setBrakeMode(true);
    // schedule the autonomous command (example)
    //next two lines of code work for now, but we'll probably want to replace them with a more 
    //elegant way of selecting the auton mode we want from the smart dashboard 
    DriveEncoders.intializeEncoders();
    RobotMap.driveTrainRightFront.setPosition(0);
    RobotMap.driveTrainLeftFront.setPosition(0);
    System.out.print(auto.getSelected());
    autonomousCommand = (CommandGroup)new AutonCommandGroup (ParseInput.takeInput((String)auto.getSelected())); 

    if (autonomousCommand != null) autonomousCommand.start();
}

public void autonomousInit() {
     RobotMap.lightRing.set(Relay.Value.kOn);
    RobotMap.winchMotor.enableBrakeMode(true);
    if (recordedAuton) {
        oi.gamepad.loadVirtualGamepad(recordedID);
        oi.gamepad.startVirtualGamepad();
    } else {
   // schedule the autonomous command (example) 
autonomousCommand = (CommandGroup) new ConstructedAutonomous(ParseInput.takeInput((String)auto_Movement.getSelected(), 
        (boolean)auto_Reverse.getSelected(), (int)auto_isHighGoal.getSelected()));
if(autonomousCommand != null)
    autonomousCommand.start();
    }
 }

/**
 * Specialization wrapper for a command group. Simply will print out that
 * the command group has finished
 * 
 * @param aName
 *            Name of the command group
 * @return The newly created command group
 */
protected CommandGroup createNewCommandGroup(String aName)
{
    return new CommandGroup(aName)
    {
        @Override
        protected void end()
        {
            super.end();
            System.out.println("Command group '" + aName + "' finished!");
        }
    };
}

/**
 * Interprets a line as a Command and adds it to mCommands
 * 
 * @param aLine
 *            Line of text
 * @param b
 */
protected void parseLine(CommandGroup aGroup, String aLine, boolean aAddParallel)
{
    aLine = aLine.trim();
    if (aLine.isEmpty() || aLine.startsWith(mCommentStart))
    {
        return;
    }

    StringTokenizer tokenizer = new StringTokenizer(aLine, mDelimiter);

    List<String> args = new ArrayList<>();

    while (tokenizer.hasMoreElements())
    {
        args.add(tokenizer.nextToken());
    }

    Command newCommand = parseCommand(args);

    if (newCommand == null)
    {
        mSuccess = false;
    }
    else
    {
        if (aAddParallel)
        {
            aGroup.addParallel(newCommand);
        }
        else
        {
            aGroup.addSequential(newCommand);
        }
    }
}

/**
 * Reads the given file into autonomous commands
 * 
 * @param aFilePath
 *            The path to the file to read
 * @return The constructed command group to run
 */
public CommandGroup readFile(String aFilePath)
{
    initReading();

    CommandGroup output = createNewCommandGroup(aFilePath);

    String fileContents = "";

    File file = new File(aFilePath);

    if (file.exists())
    {
        try
        {
            BufferedReader br = new BufferedReader(new FileReader(aFilePath));

            String line;
            while ((line = br.readLine()) != null)
            {
                this.parseLine(output, line, false);
                fileContents += line + "\n";
            }

            br.close();
        }
        catch (Exception e)
        {
            e.printStackTrace();
        }
    }
    else
    {
        addError("File " + aFilePath + " not found!");
    }

    publishParsingResults(fileContents);

    return output;
}

public CommandGroup createAutonMode()
{
    File selectedFile = mAutonModeChooser.getSelected();
    if (selectedFile != null)
    {
        setPosition();
        return mCommandParser.readFile(selectedFile.toString());
    }

    return null;

}

@Override
public CommandGroup readFile(String aFilePath)
{
    if (aFilePath == null)
    {
        aFilePath = "NOT FOUND!";
    }

    mAutonTable.putString(SmartDashBoardNames.sAUTON_FILENAME, aFilePath);
    return super.readFile(aFilePath);
}

@Override
public CommandGroup readFile(String aFilePath)
{
    if (aFilePath == null)
    {
        aFilePath = "NOT FOUND!";
    }

    mAutonTable.putString(SmartDashBoardNames.sAUTON_FILENAME, aFilePath);
    return super.readFile(aFilePath);
}

/**
 * Specialization wrapper for a command group. Simply will print out that
 * the command group has finished
 * 
 * @param aName
 *            Name of the command group
 * @return The newly created command group
 */
protected CommandGroup createNewCommandGroup(String aName)
{
    return new CommandGroup(aName)
    {
        @Override
        protected void end()
        {
            super.end();
            System.out.println("Command group '" + aName + "' finished!");
        }
    };
}

/**
 * Interprets a line as a Command and adds it to mCommands
 * 
 * @param aLine
 *            Line of text
 * @param b
 */
protected void parseLine(CommandGroup aGroup, String aLine, boolean aAddParallel)
{
    aLine = aLine.trim();
    if (aLine.isEmpty() || aLine.startsWith(mCommentStart))
    {
        return;
    }

    StringTokenizer tokenizer = new StringTokenizer(aLine, mDelimiter);

    List<String> args = new ArrayList<>();

    while (tokenizer.hasMoreElements())
    {
        args.add(tokenizer.nextToken());
    }

    Command newCommand = parseCommand(args);

    if (newCommand == null)
    {
        mSuccess = false;
    }
    else
    {
        if (aAddParallel)
        {
            aGroup.addParallel(newCommand);
        }
        else
        {
            aGroup.addSequential(newCommand);
        }
    }
}

/**
 * Reads the given file into autonomous commands
 * 
 * @param aFilePath
 *            The path to the file to read
 * @return The constructed command group to run
 */
public CommandGroup readFile(String aFilePath)
{
    initReading();

    CommandGroup output = createNewCommandGroup(aFilePath);

    String fileContents = "";

    File file = new File(aFilePath);

    if (file.exists())
    {
        try
        {
            BufferedReader br = new BufferedReader(new FileReader(aFilePath));

            String line;
            while ((line = br.readLine()) != null)
            {
                this.parseLine(output, line, false);
                fileContents += line + "\n";
            }

            br.close();
        }
        catch (Exception e)
        {
            e.printStackTrace();
        }
    }
    else
    {
        addError("File " + aFilePath + " not found!");
    }

    publishParsingResults(fileContents);

    return output;
}

@Override
protected void execute() {
  double xRate = 0;
  double yRate = 0;
  double zRate = 0;

  CommandGroup group = getGroup();
  if (group instanceof DriveStraightCommand) {
    xRate = ((DriveStraightCommand) group).getxRate();
    yRate = ((DriveStraightCommand) group).getyRate();
    zRate = ((DriveStraightCommand) group).getzRate();
  }

  Robot.driveSubsystem.mecanumDrive(xRate, -yRate, zRate, 0);
}

@Override
protected void usePIDOutput(double output) {
  CommandGroup group = getGroup();
  if (group instanceof DriveStraightCommand) {
    ((DriveStraightCommand) group).setyRate(output);
  }
}

public AutonThruPortcullis() {
addParallel(new DriveStraight(-16.0 * RATIO, 0.0, 0.5));
addSequential(new DeployArms(-800.0));
addSequential(new DriveStraight(-16 * RATIO, 0.0, 0.5));
addParallel(new StowArms());
addSequential(new CommandGroup(){
    {
        addSequential(new DriveStraight(4.5 * RATIO, 0.0, 0.5));
        addSequential(new DriveStraight(-30.0 * RATIO, 0.0, 0.6));
    }
});
  }

@Override
  public void autonomousInit() {
      // schedule the autonomous command (example)

    Object selected = autonomousChooser.getSelected();
if (selected instanceof CommandGroup) {
    autonomousCommand = ((CommandGroup) selected);
    Scheduler.getInstance().add(autonomousCommand);
}

compressor.stop();
enableSubsystems();
  }

public static Command shootHotGoalShortDriveAutonomous() {
    CommandGroup driveAndCheckGoal = driveAndPrepareToShoot(true, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT, 0, 2.5);
    CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
    //drive and check goal. When both are done (checking goal and driving), shoot
    mainAutonomousSequence.addSequential(setFingerPosition(ClawFingerSubsystem.DOWN));
    mainAutonomousSequence.addSequential(new SetClawWinchSolenoid(true));
    mainAutonomousSequence.addSequential(driveAndCheckGoal);
    mainAutonomousSequence.addSequential(new WaitCommand(200));
    mainAutonomousSequence.addSequential(shootBall());
    return mainAutonomousSequence;
}

public static Command twoBallShortDriveAutonomous() {
    CommandGroup mainAutonomousSequence = new CommandGroup("mainAutoSeq");
    mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT, 0, 1.6));
    mainAutonomousSequence.addSequential(shootBall(false));
    mainAutonomousSequence.addParallel(autoCoilClawWinch(), ClawWinchSubsystem.MAX_COIL_TIME);
    mainAutonomousSequence.addSequential(repositionAndPickup(DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT));
    mainAutonomousSequence.addParallel(realignBall());
    mainAutonomousSequence.addSequential(driveAndPrepareToShoot(false, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_SHORT - 16, 9.2, 10));
    mainAutonomousSequence.addSequential(shootBall());
    return mainAutonomousSequence;
}

private static CommandGroup realignBall() {
    CommandGroup realign = new CommandGroup();
    realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_REALIGN_SPEED));
    realign.addSequential(new WaitCommand(4000));
    realign.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_OFF_SPEED));
    return realign;
}

public static Command waitAndDriveAutonomous() {
        CommandGroup group = new CommandGroup("waitAndDrive");
//        group.addSequential(new WaitCommand(5000));
//        group.addSequential(new DriveSetDistanceByTimeCommand(DriveTrainSubsystem.TimeBasedDriving.DRIVE_SPEED, DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE));

        group.addSequential(new DriveSetDistanceWithPIDCommand(DriveTrainSubsystem.EncoderBasedDriving.AUTONOMOUS_DRIVE_DISTANCE_LONG));
        return group;
    }

public static Command shootBall(boolean auto) {
    CommandGroup fireSequence = new CommandGroup();
    fireSequence.addParallel(setFingerPosition(ClawFingerSubsystem.UP));
    fireSequence.addParallel(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_SHOOT_SPEED));
    fireSequence.addSequential(new SetClawWinchSolenoid(false));
    fireSequence.addSequential(new WaitCommand(ClawWinchSubsystem.TIME_TO_FIRE));
    //then recoils
    fireSequence.addSequential(setFingerPosition(ClawFingerSubsystem.DOWN));
    fireSequence.addSequential(new RunRollers(ClawRollerSubsystem.ROLLER_SPIN_OFF_SPEED));
    if (auto) {
        fireSequence.addSequential(autoCoilClawWinch(), ClawWinchSubsystem.MAX_COIL_TIME);
    }
    return fireSequence;

}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit(){
    enableWatchdog(true);
    compressor(DIO_COMPRESSOR, RELAY_COMPRESSOR);

    new AutonMode(){
        CommandGroup auton = new CG_OneBall();
        public void init() {auton.start();}
        public void run() {}
        public void end() {auton.cancel();}
    };
}

protected void initDefaultCommand() {
    // This command makes the shooter automatically time out if no command
    // uses it for 5 seconds
    CommandGroup timeout = new CommandGroup("Time out shooter");
    timeout.addSequential(new DoNothing(),5);
    timeout.addSequential(new SpinDown());
    setDefaultCommand(timeout);
}

public void run() {
    TestCommand command1 = new TestCommand();
    TestCommand command2 = new TestCommand();

    CommandGroup commandGroup = new CommandGroup();
    commandGroup.addParallel(command1);
    commandGroup.addParallel(command2);


    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    commandGroup.start();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 0, 0);
    assertCommandState(command2, 1, 2, 2, 0, 0);
    command1.setHasFinished(true);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 3, 3, 1, 0);
    assertCommandState(command2, 1, 3, 3, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 3, 3, 1, 0);
    assertCommandState(command2, 1, 4, 4, 0, 0);
    command2.setHasFinished(true);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 3, 3, 1, 0);
    assertCommandState(command2, 1, 5, 5, 1, 0);

}

public void run() {
    TestCommand command1 = new TestCommand();
    TestCommand command2 = new TestCommand();

    CommandGroup commandGroup = new CommandGroup();
    commandGroup.addParallel(command1);
    commandGroup.addParallel(command2);


    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    commandGroup.start();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 0, 0);
    assertCommandState(command2, 1, 2, 2, 0, 0);
    commandGroup.cancel();
    assertCommandState(command1, 1, 2, 2, 0, 0);
    assertCommandState(command2, 1, 2, 2, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 0, 1);
    assertCommandState(command2, 1, 2, 2, 0, 1);

}

@JsonIgnore
abstract void setParent(CommandGroup parent);

@Override
protected CommandGroup createAutonomousCommand()
{
    return null;
}

@Override
protected CommandGroup createAutonomousCommand()
{
    return mAutonFactory.createAutonMode();
}

public Command assembleCommand() {
    CommandGroup cmd = new CommandGroup();
    cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));// Move to the start defense
    switch (startDefense()) {// Traverse the start defense
    case LOW_BAR:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case PORTCULLIS:
        cmd.addSequential(new TraversePortcullis(true));//Travels 50
        cmd.addSequential(new DriveStraight(10 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case CHEVAL_DE_FRESE:
        return null;
    case SALLY_PORT:
        return null;
    case DRAWBRIDGE:
        return null;
    case ROUGH_TERRAIN:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.3));
        break;
    case RAMPARTS:
        return null;
    case ROCK_WALL:
        cmd.addSequential(new DriveStraight(70 * (2.0 + 4.0 / 9.0), 0, 0.8));
        break;
    case MOAT:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.7));
        break;
    }
    //Move to firing position
    switch (startPosition()) {
    case(1):
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));
        cmd.addSequential(new TurnToAtRate(46, 0.5));
        break;
    case(2):
        cmd.addSequential(new DriveStraight(130 * (2.0 + 4.0 / 9.0), 0, 0.5));
        cmd.addSequential(new TurnToAtRate(46, 0.5));
        break;
    case(3):
        cmd.addSequential(new TurnToAtRate(45, 0.5));
        cmd.addSequential(new DriveStraight(45 * (2.0 + 4.0 / 9.0), 0.5));
        cmd.addSequential(new TurnToAtRate(0, 0.5));
        break;
    case(4):
        cmd.addSequential(new DriveStraight(20 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case(5):
        cmd.addSequential(new TurnToAtRate(335, 0.5));
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0.5));
        cmd.addSequential(new TurnToAtRate(0, 0.5));
        break;
    }
    cmd.addSequential(new VisionSeek());
    cmd.addSequential(new LoadBoulder());
    return cmd;
}

public CommandGroup getAutonomousCommand() {
    return currentAutonomousCommand;
}

/** Called on robot boot. */
    public void robotInit() {
        catapult   = new Catapult();
        driveTrain = new DriveTrain();
        leds       = new LEDStrip();
        intake     = new Intake();
        ledring    = new LEDRing();
        staticleds = new StaticLEDStrip();
        compressor = new Compressor(RobotMap.PORT_SWITCH_COMPRESSO, RobotMap.PORT_RELAY_COMPRESSOR);
        compressor.start();

        // Initialize OI last so it doesn't try to access null subsystems
        oi         = new OI();

        //SmartDashboard.putBoolean("Wait longer", true);
        SmartDashboard.putData("Arms out", new SetArmPosition(2));
        SmartDashboard.putData("Arms in", new SetArmPosition(0));
//        SmartDashboard.putData("Prefire", new PreFire());
        CommandGroup armsMoveWait = new CommandGroup();
        armsMoveWait.addSequential(new PrintCommand("Arms up"));
        armsMoveWait.addSequential(new SetArmPosition(0, false));
        armsMoveWait.addSequential(new PrintCommand("Arms are up"));
        armsMoveWait.addSequential(new WaitCommand(0.5));
        armsMoveWait.addSequential(new PrintCommand("Arms down"));
        armsMoveWait.addSequential(new SetArmPosition(2, false));
        armsMoveWait.addSequential(new PrintCommand("Arms are down"));
        SmartDashboard.putData("Arms move wait", armsMoveWait);
        CommandGroup armsMoveNoWait = new CommandGroup();
        armsMoveNoWait.addSequential(new SetArmPosition(0, false));
        armsMoveNoWait.addSequential(new SetArmPosition(2, false));
        SmartDashboard.putData("Arms move no wait", armsMoveNoWait);
        SmartDashboard.putData("Arms in quick", new SetArmPosition(0,false));

        // The names, and corresponding Commands of our autonomous modes
        autonomiceNames = new String[]{"Drive Forward","1 Ball Hot","1 Ball Blind","2 Ball"};
        autonomice = new Command[]{new DriveForward(0.8, 5250),new DriveAndShoot(),new DriveAndShootNoWait(),new DriveAndShoot2Ball()};

        // Configure and send the SendableChooser, which allows autonomous modes
        // to be chosen via radio button on the SmartDashboard
        System.out.println(autonomice.length + " autonomice");
        for (int i = 0; i < autonomice.length; ++i) {
            chooser.addObject(autonomiceNames[i], autonomice[i]);
        }
        SmartDashboard.putData("Which Autonomouse?", chooser);
        SmartDashboard.putData(Scheduler.getInstance());

        /*SmartDashboard.putData("Test conditional", new Conditional(new WaitCommand(0.5), new WaitCommand(3.0)) {
            protected boolean condition() {
               return SmartDashboard.getBoolean("Wait longer", false);
            }
        });*/
        // Send sensor info to the SmartDashboard periodically
        new Command("Sensor feedback") {
            protected void initialize() {}
            protected void execute() {
                sendSensorData();
            }
            protected boolean isFinished() {
                return false;
            }
            protected void end() {}
            protected void interrupted() {
                end();
            }
        }.start();
        leds.initTable(NetworkTable.getTable("SmartDashboard"));
        ledring.initTable(NetworkTable.getTable("SmartDashboard"));
        staticleds.initTable(NetworkTable.getTable("SmartDashboard"));
    }

public static CommandGroup doNothingAutonomous() {
    return new CommandGroup();
}

/**
 * This function is called once at the start of autonomous mode.
 */
public void autonomousInit(){
    DriverStation driverStation = DriverStation.getInstance();
    double delayTime = driverStation.getAnalogIn(1);

    boolean trackHighGoal = driverStation.getDigitalIn(1);
    boolean trackMiddleGoal = driverStation.getDigitalIn(2);
    boolean shootInAuto = true;

    betweenModes();
    DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
    drivetrainStrafe.setDefaultCommand(new MaintainStateCommand(drivetrainStrafe));
    DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
    drivetrainRotation.setDefaultCommand(new MaintainStateCommand(drivetrainRotation));

    CommandGroup fastAugerSequence = new CommandGroup();
    fastAugerSequence.addSequential(new PrintCommand("Dispensing auger"));
    fastAugerSequence.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    fastAugerSequence.addSequential(new WaitCommand(0.8));

    CommandGroup augerSequence = new CommandGroup();
    augerSequence.addSequential(new PrintCommand("Dispensing auger"));
    augerSequence.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    augerSequence.addSequential(new WaitCommand(2));

    CommandGroup firstAugerDrop = new CommandGroup();
    firstAugerDrop.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    firstAugerDrop.addSequential(new WaitCommand(0.5));
    firstAugerDrop.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));

    CommandGroup autoCommand = new CommandGroup();
    autoCommand.addSequential(new PrintCommand("Starting autonomous"));
    autoCommand.addSequential(new WaitCommand(delayTime));
    if(shootInAuto){
        autoCommand.addSequential(new FixedPointVisionTrackingCommand(FixedPointVisionTrackingCommand.PYRAMID_BACK_MIDDLE, TargetType.HIGH_GOAL), 4);
        autoCommand.addParallel(firstAugerDrop);
        autoCommand.addSequential(new SetShooterCommand(Shooter.SHOOTER_ON));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new SetConveyorCommand(Conveyor.CONVEYOR_SHOOT_IN));
        autoCommand.addSequential(new WaitCommand(1));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new WaitForChildren());
        autoCommand.addSequential(new WaitCommand(2));
    }
    //autoCommand.addSequential(new RepeatCommand(new PrintCommand("Testing print"), 5));
    //autoCommand.addSequential(augerSequence, 5);

    autonomousCommand = autoCommand;
    autonomousCommand.start();
}

public void run() {
    final ASubsystem subsystem = new ASubsystem();


    TestCommand command1 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    TestCommand command2 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    TestCommand command3 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    CommandGroup commandGroup = new CommandGroup();
    commandGroup.addSequential(command1);
    commandGroup.addSequential(command2);
    commandGroup.addSequential(command3);


    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    commandGroup.start();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    command1.setHasFinished(true);
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);

    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    command2.setHasFinished(true);
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);

    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 1, 1, 0, 0);
    command3.setHasFinished(true);
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 1, 1, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 2, 2, 1, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 2, 2, 1, 0);

}