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