Java CommandGroup.addSequential方法代码示例

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包/类
/**
 * 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包/类
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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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;
    }
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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;

}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
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;
}
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
/** 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"));
    }
 

import edu.wpi.first.wpilibj.command.CommandGroup; //导入方法依赖的package包/类
/**
 * 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();
}