Autonomous *Autonomous::create1Ball(float distance) {
Autonomous *cmd = new Autonomous("1Ball");
CommandGroup *drive = new CommandGroup("Drive");
CommandGroup *prepare = new CommandGroup("CHILD");
prepare->AddParallel(new PrepareShooter(AUTO_1_SHOT_POWER));
prepare->AddSequential(new AngelChange(0,2));
prepare->AddSequential((new AngelChange(AUTO_1_SHOT_ANGLE,5)));
// prepare->AddSequential(new JawMove(Collector::kOpen, 1));
drive->AddParallel(prepare);
drive->AddSequential(new AutoDriveDistance(distance,2,5)); //All of these magic number need to be less magic
cmd->AddSequential(drive);
cmd->AddSequential(new WaitCommand(0.5));
// Fire Shooter Internal
cmd->AddSequential(new DiscBrake(Pterodactyl::kActive));
cmd->AddSequential(new WLatch(Shooter::kLatched));
cmd->AddSequential(new HotGoalWait());
cmd->AddSequential(new JawMove(Collector::kOpen, 0.5));
cmd->AddSequential(new SLatch(Shooter::kUnlatched));
cmd->AddSequential(new WaitCommand(1.0));
cmd->AddParallel(new AngelChange(0));
cmd->AddParallel(new DrawShooter());
return cmd;
}
void OI::registerButtonListeners() {
// Drivebase
SAFE_BUTTON(shiftButton,shiftButton->WhenPressed(new Shift(Shift::kLow)));
SAFE_BUTTON(shiftButton,shiftButton->WhenReleased(new Shift(Shift::kHigh)));
// Pterodactyl Angle
SAFE_BUTTON(angleFloor,angleFloor->WhenPressed(new AngelChange(0)));
SAFE_BUTTON(angleLow,angleLow->WhenPressed(new AngelChange(30)));
CommandGroup *startCfgCmd = new CommandGroup();
startCfgCmd->AddSequential(new AngelChange(111.5));
startCfgCmd->AddParallel(new JawMove(Collector::kClosed));
SAFE_BUTTON(startConfig,startConfig->WhenPressed(startCfgCmd));
// Collector rollers
SAFE_BUTTON(gulp,gulp->WhenPressed(new Gulp()));
//new RollerRoll(-COLLECTOR_ROLLER_INTAKE_SET_POINT));
SAFE_BUTTON(collect,START_STOP_COMMAND(collect, new Collect(), 1));
SAFE_BUTTON(collectButton,START_STOP_COMMAND(collectButton, new Collect(), 1));
// Jaw Operations
SAFE_BUTTON(pass, pass->WhenPressed(new Pass()));
SAFE_BUTTON(catch1, catch1->WhenPressed(new Catch(90)));
// Shooter operations
SAFE_BUTTON(fire,fire->WhenReleased(new FireShooter()));
SmartDashboard::PutData("Shoot!!!!", new FireShooter());
//fire->WhenPressed(new CommandStarter(Shooter::createArmShooter, true));
// Strap operations
SAFE_BUTTON(shotFeederTruss,shotFeederTruss->WhenPressed(new ReadyShot(FEEDER_TRUSS_SHOT_POWER, FEEDER_TRUSS_SHOT_ANGLE, FEEDER_TRUSS_SHOT_TOLERANCE, FEEDER_TRUSS_SHOT_DELAY)));
SAFE_BUTTON(shotNearTruss,shotNearTruss->WhenPressed(new ReadyShot(NEAR_TRUSS_SHOT_POWER, NEAR_TRUSS_SHOT_ANGLE, NEAR_TRUSS_SHOT_TOLERANCE, NEAR_TRUSS_SHOT_DELAY)));
SAFE_BUTTON(shotNear,shotNear->WhenPressed(new ReadyShot(NEAR_SHOT_POWER, NEAR_SHOT_ANGLE)));
SAFE_BUTTON(shotSteep,shotSteep->WhenPressed(new ReadyShot(STEEP_SHOT_POWER, STEEP_SHOT_ANGLE)));
SAFE_BUTTON(shotIRS, shotIRS->WhenPressed(new ReadyShot(IRS_SHOT_POWER, IRS_SHOT_ANGLE, IRS_SHOT_TOLERANCE)));
// Jaw Override
SAFE_BUTTON(jawToggle,jawToggle->WhenPressed(new JawMove(Collector::kClosed)));
SAFE_BUTTON(jawToggle,jawToggle->WhenReleased(new JawMove(Collector::kOpen)));
SAFE_BUTTON(resetShooter,resetShooter->WhenPressed(new ResetShooter()));
SmartDashboard::PutData("go power target", new CommandStarter(OI::createPower));
SmartDashboard::PutData("go target", new CommandStarter(OI::createAngle));
}
// CommandParallelGroupTest ported from CommandParallelGroupTest.java
TEST_F(CommandTest, ParallelCommands) {
MockCommand command1;
MockCommand command2;
CommandGroup 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);
TeardownScheduler();
}