int main()
{
int i, j, k, n, m, ins;
scanf("%d %d", &n, &m);
scanf("%s", s);
while (m--)
{
scanf("%s %d", cmd, &ins);
switch (ins)
{
case 1: scanf("%d %d %c", &i, &j, &c); cmd1(i, j, c); break;
case 2: scanf("%d %d %d", &i, &j, &k); cmd2(i, j, k); break;
case 3: scanf("%d", &k); cmd3(k, n); break;
case 4: scanf("%d %d", &i, &j); cmd4(i, j); break;
}
}
printf("%s\n", s);
return 0;
}
int ACE_TMAIN (int argc, ACE_TCHAR** argv)
{
Kokyu::ConfigInfoSet config_info(3);
int hi_prio, me_prio, lo_prio;
int sched_policy=ACE_SCHED_FIFO;
Kokyu::Dispatcher_Attributes attrs;
if (parse_args (argc, argv) == -1)
return 0;
if (ACE_OS::strcasecmp(sched_policy_str.c_str(), "fifo") == 0)
{
sched_policy = ACE_SCHED_FIFO;
}
else if (ACE_OS::strcasecmp(sched_policy_str.c_str(), "other") == 0)
{
sched_policy = ACE_SCHED_OTHER;
}
else if (ACE_OS::strcasecmp(sched_policy_str.c_str(), "rr") == 0)
{
sched_policy = ACE_SCHED_RR;
}
attrs.sched_policy (sched_policy);
hi_prio = ACE_Sched_Params::priority_max (sched_policy);
me_prio = ACE_Sched_Params::previous_priority (sched_policy,
hi_prio);
lo_prio = ACE_Sched_Params::previous_priority (sched_policy,
me_prio);
config_info[0].preemption_priority_ = 1;
config_info[0].thread_priority_ = hi_prio ;
config_info[0].dispatching_type_ = Kokyu::FIFO_DISPATCHING;
config_info[1].preemption_priority_ = 2;
config_info[1].thread_priority_ = me_prio;
config_info[1].dispatching_type_ = Kokyu::FIFO_DISPATCHING;
config_info[2].preemption_priority_ = 3;
config_info[2].thread_priority_ = lo_prio;
config_info[2].dispatching_type_ = Kokyu::FIFO_DISPATCHING;
attrs.config_info_set_ = config_info;
ACE_DEBUG ((LM_DEBUG, "before create_dispatcher\n" ));
auto_ptr<Kokyu::Dispatcher>
disp (Kokyu::Dispatcher_Factory::create_dispatcher (attrs));
ACE_ASSERT (disp.get() != 0);
MyCommand cmd1(1), cmd2(2), cmd3(3);
Kokyu::QoSDescriptor qos1, qos2, qos3;
qos1.preemption_priority_ = 2;
ACE_DEBUG ((LM_DEBUG, "Priority of command1 is %d\n",
qos1.preemption_priority_));
qos2.preemption_priority_ = 3;
ACE_DEBUG ((LM_DEBUG, "Priority of command2 is %d\n",
qos2.preemption_priority_));
qos3.preemption_priority_ = 1;
ACE_DEBUG ((LM_DEBUG, "Priority of command3 is %d\n",
qos3.preemption_priority_));
if (disp->dispatch (&cmd1, qos1) == -1 ||
disp->dispatch (&cmd2, qos2) == -1 ||
disp->dispatch (&cmd3, qos3) == -1)
ACE_ERROR_RETURN ((LM_ERROR, "Error in dispatching command object\n"), -1);
if (disp->activate () == -1)
{
ACE_ERROR_RETURN ((LM_ERROR,
ACE_TEXT ("Error activating dispatcher. ")
ACE_TEXT ("You might not have superuser privileges ")
ACE_TEXT ("to run FIFO class. Try \"-p other\"\n")), -1);
}
disp->shutdown ();
ACE_DEBUG ((LM_DEBUG, "after shutdown\n"));
return 0;
}
void cmd3(char *str, char c, int index)
{
if( index != 0 )
cmd3( str, *(str+(strlen(str))-index), index-1);
*(str+index) = c;
}
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;
}
