void Metrobot::AutonomousInit(){
SetDefaultsDoNothing();
CommandGroup *group = new CommandGroup();
switch ( autonScript ){
case AUTON_SCRIPT_NONE:
break;
case AUTON_SCRIPT_1:
/*Insert code here. Example: */
group->AddSequential( new DriveAuton( 0.4, 0.0, 200, 2.0 ) );
group->AddSequential( new DriveAuton( 0.0, 90.0, 0.0, 2.0 ) );
group->AddSequential( new DriveAuton( -0.4, 0.0, -200, 2.0 ) );
break;
case AUTON_SCRIPT_2:
break;
}
autonCommand_ = group;
autonCommand_->Start();
}
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;
}
Command *Shooter::createArmShooter() {
if (CommandBase::oi != NULL && CommandBase::oi->isShooterArmingPrevented()) {
return NULL;
}
#if SHOOTER_FANCY_LETOUT
if (CommandBase::shooter != NULL
&& CommandBase::shooter->lastReleasePosition
> SHOOTER_WENCH_POT_REVERSE_ALLOW) {
return new ReadyShot(CommandBase::shooter->lastReleasePosition);
}
#endif
CommandGroup *group = new CommandGroup("DrawSeq");
group->AddSequential(new WaitCommand(1.5));
group->AddSequential(new DrawShooter());
return group;
}
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));
}
Command *Shooter::createCreateArmShooter() {
CommandGroup *createGroup = new CommandGroup("CreateShooter");
createGroup->AddSequential(new WaitCommand(0.25));
createGroup->AddSequential(new CommandStarter(Shooter::createArmShooter, false, 1000.0));
return createGroup;
}
// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
TEST_F(CommandTest, ThreeCommandOnSubSystem) {
ASubsystem subsystem("Three Command Test Subsystem");
MockCommand command1;
command1.Requires(&subsystem);
MockCommand command2;
command2.Requires(&subsystem);
MockCommand command3;
command3.Requires(&subsystem);
CommandGroup commandGroup;
commandGroup.AddSequential(&command1, 1.0);
commandGroup.AddSequential(&command2, 2.0);
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);
Wait(1); // command 1 timeout
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 1, 1, 0, 0);
AssertCommandState(command3, 0, 0, 0, 0, 0);
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 0);
AssertCommandState(command3, 0, 0, 0, 0, 0);
Wait(2); // command 2 timeout
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 1);
AssertCommandState(command3, 1, 1, 1, 0, 0);
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 1);
AssertCommandState(command3, 1, 2, 2, 0, 0);
command3.SetHasFinished(true);
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 1);
AssertCommandState(command3, 1, 2, 2, 0, 0);
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 1);
AssertCommandState(command3, 1, 3, 3, 1, 0);
Scheduler::GetInstance().Run();
AssertCommandState(command1, 1, 1, 1, 0, 1);
AssertCommandState(command2, 1, 2, 2, 0, 1);
AssertCommandState(command3, 1, 3, 3, 1, 0);
TeardownScheduler();
}