Java CommandGroup類代碼示例

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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();

}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
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;
    }
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
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();
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
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();
 	}
 }
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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!");
        }
    };
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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);
        }
    }
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
/**
 * 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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
public CommandGroup createAutonMode()
{
    File selectedFile = mAutonModeChooser.getSelected();
    if (selectedFile != null)
    {
        setPosition();
        return mCommandParser.readFile(selectedFile.toString());
    }

    return null;

}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
@Override
public CommandGroup readFile(String aFilePath)
{
    if (aFilePath == null)
    {
        aFilePath = "NOT FOUND!";
    }

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

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
@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);
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //導入依賴的package包/類
@Override
protected void usePIDOutput(double output) {
  CommandGroup group = getGroup();
  if (group instanceof DriveStraightCommand) {
    ((DriveStraightCommand) group).setyRate(output);
  }
}