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();
}
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;
}
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));
}
void FadeCurve::set_shape(QString shapeName)
{
QDomDocument doc("FadeShapes");
if (defaultShapes.contains(shapeName)) {
QFile file(":/fadeshapes");
if (!file.open(QIODevice::ReadOnly)) {
printf("Could not open fadeshapes file!!\n");
return;
}
if (!doc.setContent(&file)) {
file.close();
printf("Could not set QDomDocument content!\n");
return;
}
file.close();
} else {
// Load from custom saved fades
}
QDomElement root = doc.documentElement();
QDomNode node = root.firstChildElement(m_sType);
QDomElement fadeElement = node.firstChildElement(shapeName);
if (fadeElement.isNull()) {
printf("%s does not exist?????\n", shapeName.toAscii().data());
return;
}
CommandGroup* group = new CommandGroup(this, tr("Fade Preset"));
group->add_command(new FadeBend(this, fadeElement.attribute( "bendfactor", "0.0" ).toDouble()));
group->add_command(new FadeStrength(this, fadeElement.attribute( "strengthfactor", "0.5" ).toDouble()));
TCommand::process_command(group);
emit stateChanged();
}
void TeleopInit()
{
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != NULL)
autonomousCommand->Cancel();
driveCommand->Start();
}
void addCommand(const std::string &command, std::function<CommandResult
(ServerConnection*, std::shared_ptr<User>, const std::string&, const std::string&, Args...)> fun,
const std::string &description = "", bool displayDescription = true)
{
std::string::size_type commandEnd = command.find(' ');
std::string commandName = command.substr(0, commandEnd);
std::string rest = commandEnd == std::string::npos ? "" : command.substr(
commandEnd + 1);
// There can be errors if (commandEnd + 1) is out of range
// They will only occur if a command name string is malformated
// (has one trailing space) so be sure to not mess up the command names.
if (commandEnd == std::string::npos || command[commandEnd + 1] == '<' ||
command[commandEnd + 1] == '[')
{
// Command end point
subCommands.emplace_back(new Command<Args...>(commandName, rest,
fun, description, displayDescription));
} else
{
// New command group
// Search for existing command group with the same name
for (std::unique_ptr<AbstractCommand> &sub : subCommands)
{
CommandGroup *cmdGroup = dynamic_cast<CommandGroup*>(sub.get());
if (cmdGroup && sub->getName() == commandName)
{
cmdGroup->addCommand(rest, fun, description,
displayDescription);
return;
}
}
// Name not found -> insert a new command group
std::unique_ptr<AbstractCommand> newCmd(new CommandGroup(commandName));
CommandGroup *group = static_cast<CommandGroup*>(newCmd.get());
group->addCommand(rest, fun, description, displayDescription);
subCommands.push_back(std::move(newCmd));
}
}
// 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();
}
void AutonomousInit()
{
if (autonomousCommand != NULL)
autonomousCommand->Start();
}
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();
}
