bool CCWorldCompletionGate::openInner() {
if (canOpen()) {
forceOpen(true);
return true;
}
return false;
}
bool CCRecordGate::openInner() {
if (canOpen()) {
// There's nothing to do here... If the DesiredRecord was reached then the gate is just open.
forceOpen(true);
return true;
}
return false;
}
void CCWorldCompletionGate::onWorldCompleted(cocos2d::EventCustom *event) {
__Dictionary *eventData = (__Dictionary *)event->getUserData();
CCWorld *world = dynamic_cast<CCWorld *>(eventData->objectForKey(CCLevelUpConsts::DICT_ELEMENT_WORLD));
CC_ASSERT(world);
if (world->getId()->compare(mAssociatedWorldId->getCString()) == 0) {
forceOpen(true);
}
}
int main(int argc, char* argv[])
{
bool norelease = false;
if (YARPParseParameters::parse(argc, argv, "-norelease"))
norelease = true;
YARPString cfgFile;
if (!YARPParseParameters::parse(argc, argv, "-cfg", cfgFile))
cfgFile = __defaultCfgFile;
ReflexShared shared;
GRBehavior behavior(&shared);
GRBLoopTouch loopTouch;
GRBWaitIdle waitClose1;
GRBWaitIdle waitOpen1;
GRBWaitIdle waitOpen2;
GRBPickRndAction pickRnd;
GRBWaitDeltaT waitT[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2,
0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 };
GRBGrasp grasp;
GRBCloseOutputCommand closeCmd;
GRBOpenOutputCommand openCmd;
GRBInhibitCommand inhibitOutput;
GRBReleaseCommand enableOutput;
GRBOutputSignal sendClutching(YBVGraspRflxClutch);
GRBOutputSignal inhibitReaching(YBVReachingInhibit);
GRBOutputSignal enableReaching(YBVReachingEnable);
GRBInit init;
GRBSimpleInput motionDone(YBVHandDone);
GRBSimpleInput inhibit(YBVGraspRflxInhibit);
GRBSimpleInput release(YBVGraspRflxRelease);
GRBSimpleInput forceOpen(YBVGraspRflxForceOpen);
readConfigFile(cfgFile, shared);
behavior.setInitialState(&loopTouch);
behavior.add(&inhibit, &inhibitOutput);
behavior.add(&release, &enableOutput);
behavior.add(&init, &loopTouch, &pickRnd, &inhibitReaching);
behavior.add(NULL, &loopTouch, &loopTouch); // loopTouch is a blocking state
behavior.add(NULL, &pickRnd, &waitOpen1, &openCmd);
behavior.add(&motionDone, &waitOpen1, &grasp, &closeCmd);
behavior.add(NULL, &grasp, &waitClose1);
behavior.add(&motionDone, &waitClose1, &waitT[0], &sendClutching);
for(int i = 0; i<(N-1); i++)
behavior.add(NULL, &waitT[i], &waitT[i+1]);
// check no automatic release
if (norelease)
behavior.add(&forceOpen, &waitT[N-1], &waitOpen2, &openCmd);
else
behavior.add(NULL, &waitT[N-1], &waitOpen2, &openCmd);
behavior.add(&motionDone, &waitOpen2, &loopTouch, &enableReaching);
behavior.Begin();
behavior.loop();
return 0;
}
int main(int argc, char* argv[])
{
ExplorationShared shared;
EBehavior behavior(&shared);
EBWaitIdle waitStart("Start");
EBWaitIdle waitMotion1("ArmPositioning");
EBWaitIdle waitMotion2("Turn1");
EBWaitIdle waitMotion3("Turn2");
EBWaitIdle waitMotion4("Turn3");
EBWaitIdle waitMotion5("Turn4");
EBWaitIdle waitMotion6("Turn5");
EBWaitIdle waitMotion7("Final pos");
EBWaitIdle waitMotion8("Going back to rest");
EBWaitIdle endState1("end1");
EBWaitIdle endState2("end2");
EBWaitIdle stateInhibitHandTracking("Inhibit hand tracking");
EBWaitIdle stateEnableHandTracking("Enable hand tracking");
EBWaitIdle position1Wait("Position1 done");
EBWaitIdle position2Wait("Position2 done");
EBWaitIdle position3Wait("Position3 done");
EBWaitDeltaT position2Train(6);
EBWaitDeltaT position3Train(6);
EBWaitDeltaT position4Train(6);
EBWaitIdle position2("Waiting arm done");
EBWaitIdle position3("Waiting arm done");
EBWaitIdle position4("Waiting arm done");
EBWaitIdle waitArmAck("Wait armAck");
EBWaitDeltaT dT1(6);
EBWaitDeltaT dT2(0.2);
EBWaitDeltaT dT3OneSecond(3);
EBWaitDeltaT dTHandClosing(0.1);
EBWaitDeltaT waitArmSeemsToRest(5);
EBBehaviorOutput startTrain(YBVKFTrainStart);
EBBehaviorOutput startSequence(YBVKFStart);
EBBehaviorOutput stopTrain(YBVKFTrainStop);
EBBehaviorOutput stop(YBVKFStop);
EBSimpleOutput forceOpen(YBVGraspRflxForceOpen);
EBSimpleOutput parkArm(YBVArmForceRestingTrue);
// output: enable and disable hand tracking system
EBEnableTracker enableHandTracking;
EBInhibitTracker inhibitHandTracking;
EBOutputCommand cmd1(YBVArmForceNewCmd, YVector(_nJoints, pos1));
EBOutputCommand cmd2(YBVArmForceNewCmd, YVector(_nJoints, pos2));
EBOutputCommand cmd3(YBVArmForceNewCmd, YVector(_nJoints, pos3));
// EBOutputCommand cmd4(YBVArmForceNewCmd, YVector(_nJoints, pos4));
// EBOutputCommand cmd5(YBVArmForceNewCmd, YVector(_nJoints, pos5));
// EBOutputCommand cmd6(YBVArmForceNewCmd, YVector(_nJoints, pos6));
EBOutputCommand cmd7(YBVArmForceNewCmd, YVector(_nJoints, pos7));
EBOutputCommand cmd8(YBVArmForceNewCmd, YVector(_nJoints, pos8));
EBOutputCommand cmd9(YBVArmForceNewCmd, YVector(_nJoints, pos9));
EBOutputCommand cmd10(YBVArmForceNewCmd, YVector(_nJoints, pos10));
EBSimpleInput armDone(YBVArmDone);
EBSimpleInput handDone(YBVHandDone);
EBSimpleInput start(YBVKFExplorationStart);
EBSimpleInput start2(YBVGraspRflxClutch);
EBSimpleInput armAck(YBVArmIssuedCmd);
EBSimpleInput armNAck(YBVArmIsBusy);
behavior.setInitialState(&waitStart);
behavior.add(&start, &waitStart, &waitMotion1, &cmd1);
behavior.add(&start2, &waitStart, &dTHandClosing, &enableHandTracking);
behavior.add(NULL, &dTHandClosing, &waitArmAck, &cmd1);
behavior.add(&armAck, &waitArmAck, &stateEnableHandTracking, &startSequence);
behavior.add(&armNAck, &waitArmAck, &waitArmSeemsToRest, &inhibitHandTracking);
behavior.add(NULL, &waitArmSeemsToRest, &waitStart, &forceOpen);
behavior.add(NULL, &stateEnableHandTracking, &waitMotion1, &enableHandTracking);
behavior.add(&armDone, &waitMotion1, &dT3OneSecond);
// wait some extra time before issueing the startKF signal
behavior.add(NULL, &dT3OneSecond, &dT1, &startTrain);
// july 21/04
behavior.add(NULL, &dT1, &waitMotion6);
behavior.add(NULL, &waitMotion6, &position1Wait, &stopTrain);
// position2
behavior.add(NULL, &position1Wait, &position2, &cmd7);
behavior.add(&armDone, &position2, &position2Train, &startTrain);
behavior.add(NULL, &position2Train, &position2Wait, &stopTrain);
//position3
behavior.add(NULL, &position2Wait, &position3, &cmd8);
behavior.add(&armDone, &position3, &position3Train, &startTrain);
behavior.add(NULL, &position3Train, &position3Wait, &stopTrain);
//position 4
behavior.add(NULL, &position3Wait, &position4, &cmd9);
behavior.add(&armDone, &position4, &position4Train, &startTrain);
behavior.add(NULL, &position4Train, &stateInhibitHandTracking, &stopTrain);
behavior.add(NULL, &stateInhibitHandTracking, &dT2);
/*
behavior.add(NULL, &dT1, &waitMotion2, &cmd2);
// behavior.add(&armDone, &waitMotion1, &waitMotion2, &cmd2);
behavior.add(&armDone, &waitMotion2, &waitMotion3, &cmd3);
behavior.add(&armDone, &waitMotion3, &waitMotion4, &cmd4);
behavior.add(&armDone, &waitMotion4, &waitMotion5, &cmd5);
behavior.add(&armDone, &waitMotion5, &waitMotion6, &cmd6);
behavior.add(&armDone, &waitMotion6, &dT2, &stopKF);
*/
behavior.add(NULL, &dT2, &waitMotion7, &cmd10);
behavior.add(&armDone, &waitMotion7, &waitMotion8, &forceOpen);
behavior.add(&handDone, &waitMotion8, &endState1, &parkArm);
behavior.add(NULL, &endState1, &endState2, &inhibitHandTracking);
behavior.add(NULL, &endState2, &waitStart, &stop);
behavior.Begin();
behavior.loop();
return 0;
}
