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

}