Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

MMCombo.cc

Go to the documentation of this file.
00001 #include "MMCombo.h"
00002 #include "Shared/WorldState.h"
00003 #include "Shared/Profiler.h"
00004 #include "Shared/debuget.h"
00005 #include "Shared/Config.h"
00006 #include "Shared/SharedObject.h"
00007 #include "Shared/ProcessID.h"
00008 #include "Events/EventRouter.h"
00009 #include "Behaviors/BehaviorBase.h"
00010 #include "Motion/MotionManager.h"
00011 #include "SoundPlay/SoundManager.h"
00012 #include "Vision/Vision.h"
00013 #include "Events/TextMsgEvent.h"
00014 
00015 #include "Shared/ERS210Info.h"
00016 #include "Shared/ERS220Info.h"
00017 
00018 #include <OPENR/OSyslog.h>
00019 #include <OPENR/core_macro.h>
00020 #include <OPENR/OFbkImage.h>
00021 
00022 using namespace std;
00023 
00024 extern BehaviorBase& startupBehavior;
00025 
00026 MMCombo::MMCombo()
00027   : OObject(), motmanMemRgn(NULL), worldStateMemRgn(NULL),
00028     soundManagerMemRgn(NULL), eventTranslatorQueueMemRgn(NULL),
00029     runLevel(0), num_open(0), etrans(), RPOPENR_isready(true), isStopped(true)
00030 {
00031   for(unsigned int i=0; i<NumOutputs; i++) {
00032     primIDs[i]=oprimitiveID_UNDEF;
00033     open[i]=false;
00034   }
00035   Profiler::initBuckets();
00036 }
00037 
00038 
00039 OStatus
00040 MMCombo::DoInit(const OSystemEvent&)
00041 {
00042   isStopped=false;
00043 
00044   NEW_ALL_SUBJECT_AND_OBSERVER;
00045   REGISTER_ALL_ENTRY;
00046   SET_ALL_READY_AND_NOTIFY_ENTRY;
00047     
00048   // make sure the library doesn't drop data "for" us on this reliable communication channel
00049   observer[obsReceiveWorldState]->SetBufCtrlParam(0,1,1);
00050   observer[obsReceiveMotionManager]->SetBufCtrlParam(0,1,1);
00051   observer[obsMotionManagerComm]->SetBufCtrlParam(0,1,MotionManager::MAX_MOTIONS+1);
00052   //+1 to MAX_MOTIONS so we can get a delete message after we've filled up
00053     
00054   //Read config file
00055   config=new Config("/ms/config/tekkotsu.cfg");
00056 
00057   erouter = new EventRouter;
00058 
00059   if(strcmp(objectName,"MainObj")==0) {
00060     ProcessID::setID(ProcessID::MainProcess);
00061 
00062     bool isSlowOutput[NumOutputs];
00063     for(unsigned int i=0; i<NumOutputs; i++)
00064       isSlowOutput[i]=!IsFastOutput[i];
00065 
00066     SetupOutputs(isSlowOutput);
00067 
00068     //Request power status updates
00069     OPowerStatus observationStatus;
00070     observationStatus.Set(orsbALL,obsbALL,opsoREMAINING_CAPACITY_NOTIFY_EVERY_CHANGE,opsoTEMPERATURE_NOTIFY_EVERY_CHANGE,opsoTIME_DIF_NOTIFY_EVERY_CHANGE,opsoVOLUME_NOTIFY_EVERY_CHANGE);
00071     OServiceEntry entry(myOID_, Extra_Entry[entryGotPowerEvent]);
00072     OStatus result = OPENR::ObservePowerStatus(observationStatus, entry);
00073     if(result != oSUCCESS) {
00074       OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoStart()","OPENR::ObservePowerStatus() FAILED", result));
00075       return oFAIL;
00076     }
00077 
00078     //Setup wireless
00079     wireless = new Wireless();
00080     sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6);
00081     serr=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*2);
00082     serr->setFlushType(SocketNS::FLUSH_BLOCKING);
00083     sout->setTextForward();
00084     serr->setForward(sout);
00085 
00086     //worldStateMemRgn -> state setup
00087     worldStateMemRgn = InitRegion(sizeof(WorldState));
00088     state=new ((WorldState*)worldStateMemRgn->Base()) WorldState;
00089 
00090     //eventTranslatorQueueMemRgn -> etrans setup
00091     eventTranslatorQueueMemRgn = InitRegion(sizeof(EventTranslator::Queue_t));
00092     EventTranslator::Queue_t * etransq=new ((EventTranslator::Queue_t*)eventTranslatorQueueMemRgn->Base()) EventTranslator::Queue_t;
00093     etrans.setQueue(etransq);
00094     MotionCommand::setQueue(etransq);
00095 
00096     //Setup vision
00097     vision=new Vision();
00098 
00099   }
00100   if(strcmp(objectName,"MotoObj")==0) {
00101     ProcessID::setID(ProcessID::MotionProcess);
00102 
00103     SetupOutputs(IsFastOutput);
00104     OPENR::SetMotorPower(opowerON);
00105     OPENR::EnableJointGain(oprimitiveID_UNDEF); //oprimitiveID_UNDEF means enable all
00106 
00107     //motmanMemRgn -> motman setup
00108     motmanMemRgn = InitRegion(sizeof(MotionManager));
00109     motman = new (motmanMemRgn->Base()) MotionManager();
00110     motman->InitAccess(subject[sbjMotionManagerComm]);
00111   }
00112   startupBehavior.AddReference();
00113   return oSUCCESS;
00114 }
00115 
00116 OStatus
00117 MMCombo::DoStart(const OSystemEvent&)
00118 {
00119   //  cout << objectName << "::DoStart() " << endl;
00120 
00121   // initialize the current power status, doesn't always give us
00122   // a power update right away otherwise
00123   if(strcmp(objectName,"MainObj")==0) {
00124     wireless->listen(sout, config->main.console_port);
00125     wireless->listen(serr, config->main.stderr_port);
00126     OPowerStatus power;
00127     OPENR::GetPowerStatus(&power);
00128     state->read(power,erouter);
00129   }
00130 
00131   isStopped=false;
00132 
00133   ENABLE_ALL_SUBJECT;
00134   ASSERT_READY_TO_ALL_OBSERVER;
00135 
00136   addRunLevel();
00137   
00138   return oSUCCESS;
00139 }
00140 
00141 OStatus
00142 MMCombo::DoStop(const OSystemEvent&)
00143 {
00144   cout << objectName << "::DoStop()..." << endl;
00145   if(strcmp(objectName,"MainObj")==0) {
00146     startupBehavior.DoStop();
00147     wireless->close(sout);
00148     wireless->close(serr);
00149   }
00150   DISABLE_ALL_SUBJECT;
00151   DEASSERT_READY_TO_ALL_OBSERVER;
00152   isStopped=true;
00153   cout << objectName << "::DoStop()-DONE" << endl;
00154   return oSUCCESS;
00155 }
00156 
00157 OStatus
00158 MMCombo::DoDestroy(const OSystemEvent&)
00159 {
00160   cout << objectName << "::DoDestroy()..." << endl;
00161   startupBehavior.RemoveReference();
00162   if(strcmp(objectName,"MainObj")==0) {
00163     delete erouter;
00164     motmanMemRgn->RemoveReference();
00165   }
00166   if(strcmp(objectName,"MotoObj")==0) {
00167     worldStateMemRgn->RemoveReference();
00168     eventTranslatorQueueMemRgn->RemoveReference();
00169   }
00170   soundManagerMemRgn->RemoveReference();
00171   DELETE_ALL_SUBJECT_AND_OBSERVER;
00172   cout << objectName << "::DoDestroy()-DONE" << endl;
00173   return oSUCCESS;
00174 }
00175 
00176 /*! Called when MotoObj is initially ready as well as when it has finished
00177  *  processing the previous message - we only want to do this the first time
00178  *  otherwise we infinite loop. */
00179 void
00180 MMCombo::ReadyRegisterWorldState(const OReadyEvent&){
00181   static bool is_init=true;
00182   if(is_init) {
00183     is_init=false;
00184     cout << objectName << " Registering WorldState" << endl;
00185     if(strcmp(objectName,"MainObj")==0) {
00186       subject[sbjRegisterWorldState]->SetData(worldStateMemRgn);
00187       subject[sbjRegisterWorldState]->NotifyObservers();
00188     }
00189   }
00190 }
00191 
00192 void
00193 MMCombo::GotWorldState(const ONotifyEvent& event){
00194   cout << objectName << "-GOTWORLDSTATE..." << flush;
00195   //  PROFSECTION("GotMemRegion()",state->mainProfile);
00196   if(strcmp(objectName,"MotoObj")==0) {
00197     ASSERT(event.NumOfData()==1,"Too many WorldStates");
00198     worldStateMemRgn = event.RCData(0);
00199     worldStateMemRgn->AddReference();
00200     state = reinterpret_cast<WorldState*>(worldStateMemRgn->Base());
00201   }
00202   observer[obsReceiveWorldState]->AssertReady();
00203   cout << "done" << endl;
00204 }
00205 
00206     
00207 /*! Called when MainObj is initially ready as well as when it has finished
00208  *  processing the previous message - we only want to do this the first time
00209  *  otherwise we infinite loop. */
00210 void
00211 MMCombo::ReadyRegisterMotionManager(const OReadyEvent&){
00212   static bool is_init=true;
00213   if(is_init) {
00214     is_init=false;
00215     cout << objectName << " Registering MotionManager" << endl;
00216     if(strcmp(objectName,"MotoObj")==0) {
00217       subject[sbjRegisterMotionManager]->SetData(motmanMemRgn);
00218       subject[sbjRegisterMotionManager]->NotifyObservers();
00219     }
00220   }
00221 }
00222 
00223 void
00224 MMCombo::GotMotionManager(const ONotifyEvent& event){
00225   cout << objectName << "-GOTWORLDSTATE..." << flush;
00226   //  PROFSECTION("GotMemRegion()",state->motoProfile);
00227   if(strcmp(objectName,"MainObj")==0) {
00228     ASSERT(event.NumOfData()==1,"Too many MotionManagers");
00229     motmanMemRgn = event.RCData(0);
00230     motmanMemRgn->AddReference();
00231     motman = reinterpret_cast<MotionManager*>(motmanMemRgn->Base());
00232     cout << "MAIN INIT MOTMAN..." << flush;
00233     //      hexout(event.RCData(event_data_id)->Base(),128);
00234     motman->InitAccess(subject[sbjMotionManagerComm]);
00235     addRunLevel();
00236   }
00237   observer[obsReceiveMotionManager]->AssertReady();
00238   cout << "done" << endl;
00239 }
00240 
00241 /*! Called when MotoObj is initially ready as well as when it has finished
00242  *  processing the previous message - we only want to do this the first time
00243  *  otherwise we infinite loop. */
00244 void
00245 MMCombo::ReadyRegisterEventTranslatorQueue(const OReadyEvent&){
00246   static bool is_init=true;
00247   if(is_init) {
00248     is_init=false;
00249     cout << objectName << " Registering EventTranslatorQueue" << endl;
00250     if(strcmp(objectName,"MainObj")==0) {
00251       subject[sbjRegisterEventTranslatorQueue]->SetData(eventTranslatorQueueMemRgn);
00252       subject[sbjRegisterEventTranslatorQueue]->NotifyObservers();
00253     }
00254   }
00255 }
00256 
00257 void
00258 MMCombo::GotEventTranslatorQueue(const ONotifyEvent& event){
00259   cout << objectName << "-GOTEventTranslatorQueue..." << flush;
00260   //  PROFSECTION("GotMemRegion()",state->mainProfile);
00261   if(strcmp(objectName,"MotoObj")==0) {
00262     ASSERT(event.NumOfData()==1,"Too many EventTranslatorQueue");
00263     eventTranslatorQueueMemRgn = event.RCData(0);
00264     eventTranslatorQueueMemRgn->AddReference();
00265     EventTranslator::Queue_t * etransq=reinterpret_cast<EventTranslator::Queue_t*>(eventTranslatorQueueMemRgn->Base());
00266     etrans.setQueue(etransq);
00267     MotionCommand::setQueue(etransq);
00268     erouter->addTrapper(&etrans);
00269   }
00270   observer[obsReceiveEventTranslatorQueue]->AssertReady();
00271   cout << "done" << endl;
00272 }
00273 
00274     
00275 void
00276 MMCombo::ReadySendJoints(const OReadyEvent& sysevent) {
00277 
00278   if(isStopped) {
00279     //cout << "BAH!ReadySendJoints" << endl;
00280     return;
00281   }
00282 
00283   static unsigned int id=-1U;
00284   Profiler::Timer timer;
00285   if(ProcessID::getID()==ProcessID::MotionProcess) {
00286     if(state) {
00287       if(id==-1U)
00288         id=state->motionProfile.getNewID("ReadySendJoints()");
00289       timer.setID(id,&state->motionProfile);
00290     }
00291   } else if(ProcessID::getID()==ProcessID::MainProcess) {
00292     if(id==-1U)
00293       id=state->mainProfile.getNewID("ReadySendJoints()");
00294     timer.setID(id,&state->mainProfile);
00295   }
00296 
00297   if(num_open==0) //If we don't have any joints to open, leave now. (i.e. MainObj on a 220, has no ears)
00298     return;
00299 
00300   // Find an unused command vector
00301   RCRegion* rgn=NULL;
00302   for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00303     if (region[i]->NumberOfReference() == 1) {
00304       rgn=region[i];
00305       /*      if(strcmp(objectName,"MainObj")==0) {
00306               static unsigned int lasttime=get_time();
00307               unsigned int thistime=get_time();
00308               cout << '*' << i << ' ' << thistime << '\t' << (thistime-lasttime) << endl;
00309               lasttime=thistime;
00310               }*/
00311       break;
00312     }
00313   }
00314   ASSERTRET(rgn!=NULL,"Could not find unused command vector");
00315   ASSERTRET(rgn->Base()!=NULL,"Bad Command Vector");
00316   OCommandVectorData* cmdVecData = reinterpret_cast<OCommandVectorData*>(rgn->Base());
00317   
00318   // Update the outputs (note that Main is doing the ears)
00319   //I'm using an id compare instead of the slightly more readable strcmp for a tiny bit of speed
00320   if(ProcessID::getID()==ProcessID::MotionProcess) {
00321     float outputs[NumFrames][NumOutputs];
00322     if(state!=NULL) {
00323       motman->getOutputs(outputs);
00324       motman->updatePIDs(primIDs);
00325       motman->updateWorldState();
00326     } else {
00327       for(unsigned int f=0; f<NumFrames; f++)
00328         for(unsigned int i=0; i<NumOutputs; i++)
00329           outputs[f][i]=0;
00330     }
00331 
00332     // Should be a relatively simple matter to copy angles into commands...
00333     unsigned int used=0; //but only copy open joints (so main does ears, motion does everything else)
00334     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00335       if(open[i]) {
00336         OJointCommandValue2* jval = reinterpret_cast<OJointCommandValue2*>(cmdVecData->GetData(used)->value);
00337         for(unsigned int frame=0; frame<NumFrames; frame++)
00338           jval[frame].value = (slongword)(outputs[frame][i]*1e6);
00339         used++;
00340       }
00341     for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00342       if(open[i]) {
00343         OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00344         for(unsigned int frame=0; frame<NumFrames; frame++)
00345           jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00346         used++;
00347       }
00348     for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00349       if(open[i]) {
00350         OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00351         for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00352           jval[frame].value = (outputs[frame][i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00353         used++;
00354       }
00355   } else if(ProcessID::getID()==ProcessID::MainProcess) {
00356     // Just copy over the current ear state from WorldState in case this is in Main
00357     unsigned int used=0; //but only copy open joints (so main does ears, motion does everything else)
00358     for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00359       if(open[i]) {
00360         OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00361         for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00362           jval[frame].value = (state->outputs[i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00363         used++;
00364       }
00365   }
00366 
00367   // Send outputs to system
00368   subject[sbjMoveJoint]->SetData(rgn);
00369 
00370   // The first time this is called, we actually need to send *two* buffers
00371   // in order to get the double buffering going... (well, actually generalized
00372   // for NUM_COMMAND_VECTOR level buffering)
00373   static unsigned int initCount=1;
00374   if(initCount<NUM_COMMAND_VECTOR) {
00375     initCount++;
00376     ReadySendJoints(sysevent);
00377   } else //recursive base case
00378     subject[sbjMoveJoint]->NotifyObservers();
00379 }
00380 
00381 void
00382 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00383   //  if(state && state->buttons[RFrPawOffset])
00384   //  cout << "SENSOR..."<<flush;
00385   if(isStopped) {
00386     //cout << "BAH!GotSensorFrame" << endl;
00387     return;
00388   }
00389 
00390   PROFSECTION("GotSensorFrame()",state->mainProfile);
00391   etrans.translateEvents();
00392 
00393   OSensorFrameVectorData* rawsensor = reinterpret_cast<OSensorFrameVectorData*>(event.RCData(0)->Base());
00394   state->read(rawsensor[0],erouter);
00395   erouter->processTimers();
00396   static unsigned int throwaway=1; //i thought the first few sensor updates might be flakey, but now i think not.  But a good way to delay startup.
00397   if(throwaway!=0) {
00398     throwaway--;
00399     if(throwaway==0)
00400       addRunLevel();
00401   }
00402   observer[obsSensorFrame]->AssertReady();
00403   if (state!=NULL && state->wsser!=NULL)
00404     state->wsser->serialize();
00405   //  if(state && state->buttons[RFrPawOffset])
00406   //  cout << "done" << endl;
00407 }
00408 
00409 void
00410 MMCombo::GotImage(const ONotifyEvent& event){
00411   if(isStopped) {
00412     //cout << "BAH!GotImage" << endl;
00413     return;
00414   }
00415 
00416   PROFSECTION("GotImage()",state->mainProfile);
00417   etrans.translateEvents();
00418 
00419   const OFbkImageVectorData *fbkImageVectorData=reinterpret_cast<const OFbkImageVectorData*>(event.Data(0));
00420   OFbkImageInfo* info = const_cast<OFbkImageVectorData*>(fbkImageVectorData)->GetInfo(0);
00421   const byte* dat = const_cast<OFbkImageVectorData*>(fbkImageVectorData)->GetData(0);
00422   const OFbkImage yimage(info, const_cast<byte*>(dat), ofbkimageBAND_Y);
00423   const OFbkImage uimage(info, const_cast<byte*>(dat), ofbkimageBAND_Cr);
00424   const OFbkImage vimage(info, const_cast<byte*>(dat), ofbkimageBAND_Cb);
00425             
00426   const byte* bytes_y = yimage.Pointer();
00427   const byte* bytes_u = uimage.Pointer();
00428   const byte* bytes_v = vimage.Pointer();
00429 
00430   int width  = yimage.Width();
00431   int height = yimage.Height();
00432 
00433   vision->processFrame(bytes_y, bytes_u, bytes_v, width, height);
00434  
00435   erouter->processTimers();
00436   
00437   observer[obsImage]->AssertReady();
00438 }
00439 
00440 void
00441 MMCombo::GotPowerEvent(void * msg){
00442   if(isStopped) {
00443     //cout << "BAH!GotPowerEvent" << endl;
00444     return;
00445   }
00446 
00447   //  cout << "POWER..."<<flush;
00448   PROFSECTION("PowerEvent()",state->mainProfile);
00449   etrans.translateEvents();
00450 
00451   static bool first=true;
00452   if(first) {
00453     addRunLevel();
00454     first=false;
00455   }
00456   const OPowerStatus* result = &static_cast<OPowerStatusMessage*>(msg)->powerStatus;
00457   state->read(*result,erouter);
00458   erouter->processTimers();
00459   // this part watches to see if the power button is pressed to shutdown the robot
00460   // i'm leaving this low-level because there's not much else you can do anyway...
00461   // the hardware kills power to the motors, and as far as we can tell, you can't
00462   // turn them back on.
00463   if(state->powerFlags[PowerSourceID::PauseSID]) {
00464     cout << "%%%%%%%  Pause button was pushed! %%%%%%%" << endl;
00465     OBootCondition bc(0);
00466     OPENR::Shutdown(bc);
00467   }
00468   //  cout << "done" << endl;
00469 }
00470 
00471 void
00472 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00473   if(isStopped) {
00474     //cout << "BAH!GotMotionMsg" << endl;
00475     return;
00476   }
00477 
00478   //  cout << "RECEIVE..."<<flush;
00479   if(motman!=NULL)
00480     motman->receivedMsg(event);
00481   else
00482     cout << "*** WARNING Main dropping MotionCommand (motman not ready) " << endl;
00483   observer[obsMotionManagerComm]->AssertReady();
00484   //  cout << "done" << endl;
00485 }
00486 
00487 void
00488 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00489   cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00490   //  PROFSECTION("GotMemRegion()",state->mainProfile);
00491   ASSERT(event.NumOfData()==1,"Too many SoundManagers");
00492   soundManagerMemRgn = event.RCData(0);
00493   soundManagerMemRgn->AddReference();
00494   sndman = reinterpret_cast<SoundManager*>(soundManagerMemRgn->Base());
00495   observer[obsReceiveSoundManager]->AssertReady();
00496   sndman->InitAccess(subject[sbjSoundManagerComm]);
00497   addRunLevel();
00498   cout << "done" << endl;
00499 }
00500 
00501 void
00502 MMCombo::OpenPrimitives()
00503 {
00504   for(unsigned int i=0; i<NumOutputs; i++)
00505     if(open[i]) {
00506       OStatus result = OPENR::OpenPrimitive(PrimitiveName[i], &primIDs[i]);
00507       if (result != oSUCCESS)
00508         OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoInit()","OPENR::OpenPrimitive() FAILED", result));
00509     }
00510 }
00511 
00512 void
00513 MMCombo::SetupOutputs(const bool to_open[NumOutputs])
00514 {
00515   char robotDesignStr[orobotdesignNAME_MAX];
00516   memset(robotDesignStr, 0, sizeof(robotDesignStr));
00517   if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00518     cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00519     return;
00520   } else {
00521     if(strcmp(robotDesignStr,"ERS-210")==0) {
00522       for(unsigned int j=0; j<NumOutputs; j++)
00523         open[j]=to_open[j] && ERS210Info::IsRealERS210[j];
00524     } else if(strcmp(robotDesignStr,"ERS-220")==0) {
00525       for(unsigned int j=0; j<NumOutputs; j++)
00526         open[j]=to_open[j] && ERS220Info::IsRealERS220[j];
00527     } else {
00528       cout << "MMCombo::SetupOutputs - ERROR: Unrecognized model: "<<robotDesignStr<<"\nSorry..."<<endl;
00529       return;
00530     }
00531   }
00532   
00533   // count how many we're opening
00534   for(unsigned int j=0; j<NumOutputs; j++)
00535     if(open[j])
00536       num_open++;
00537 
00538   if(num_open==0) //If we don't have any joints to open, leave now. (i.e. MainObj on a 220, has no ears)
00539     return;
00540 
00541   OpenPrimitives();
00542 
00543   OStatus result;
00544   MemoryRegionID      cmdVecDataID;
00545   OCommandVectorData* cmdVecData;
00546 
00547   // request memory regions
00548   for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00549     result = OPENR::NewCommandVectorData(num_open,&cmdVecDataID,&cmdVecData);
00550     if (result != oSUCCESS)
00551       OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::NewCommandVectorData()","OPENR::NewCommandVectorData() FAILED", result));
00552     region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,cmdVecData->vectorInfo.offset,(void*)cmdVecData,cmdVecData->vectorInfo.totalSize);
00553     cmdVecData->SetNumData(num_open);
00554 
00555     // initialize the outputs we just opened
00556     unsigned int used=0;
00557     ASSERT(cmdVecData==reinterpret_cast<OCommandVectorData*>(region[i]->Base())," should be equal!?");
00558     for(unsigned int j=PIDJointOffset; j<PIDJointOffset+NumPIDJoints; j++)
00559       if(open[j]) {
00560         OCommandInfo* info = cmdVecData->GetInfo(used++);
00561         info->Set(odataJOINT_COMMAND2, primIDs[j], NumFrames);
00562       }
00563     for(unsigned int j=LEDOffset; j<LEDOffset+NumLEDs; j++)
00564       if(open[j]) {
00565         OCommandInfo* info = cmdVecData->GetInfo(used);
00566         info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00567         OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00568         for(unsigned int frame=0; frame<NumFrames; frame++)
00569           jval[frame].period = 1;
00570         used++;
00571       }     
00572     for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00573       if(open[j]) {
00574         OCommandInfo* info = cmdVecData->GetInfo(used++);
00575         info->Set(odataJOINT_COMMAND3, primIDs[j], NumSlowFrames);
00576       }     
00577   }
00578 }
00579 
00580 /*! Will round up size to the nearest page */
00581 RCRegion*
00582 MMCombo::InitRegion(unsigned int size) {
00583   unsigned int pagesize=4096;
00584   sError err=GetPageSize(&pagesize);
00585   ASSERT(err==sSUCCESS,"Error "<<err<<" getting page size");
00586   unsigned int pages=(size+pagesize-1)/pagesize;
00587   return new RCRegion(pages*pagesize);
00588 }
00589 
00590 void
00591 MMCombo::addRunLevel() {
00592   runLevel++;
00593   if(runLevel==readyLevel) {
00594     cout << "START UP BEHAVIOR..." << flush;
00595     startupBehavior.DoStart();
00596     cout << "START UP BEHAVIOR-DONE" << endl;
00597   }
00598 }
00599 
00600 void
00601 MMCombo::RPOPENR_notify(const ONotifyEvent& event) {
00602   const char *buf = (const char *)event.Data(0);
00603   observer[event.ObsIndex()]->AssertReady();
00604 
00605   /* usercode: message received
00606    * an example could be- */
00607   // RPOPENR_send("yeah i received your message", 29);
00608 
00609   /* create TextMsg event which can contain strings
00610    * comment this out if you want to handle the received data exclusively
00611    */
00612   erouter->postEvent(new TextMsgEvent(buf));
00613 }
00614 
00615 int
00616 MMCombo::RPOPENR_send(char *buf, int bufsize) {
00617   if (RPOPENR_isready && bufsize>0) {
00618     RPOPENR_isready=false;
00619     subject[sbjRPOPENRSendString]->SetData(buf, bufsize);
00620     subject[sbjRPOPENRSendString]->NotifyObservers();
00621     return bufsize;
00622   }
00623   return 0;
00624 }
00625 
00626 /*! @file
00627  * @brief Implements MMCombo, the OObject which "forks" (sort of) into Main and Motion processes
00628  * @author ejt (Creator)
00629  *
00630  * $Author: ejt $
00631  * $Name: tekkotsu-1_4_1 $
00632  * $Revision: 1.36 $
00633  * $State: Exp $
00634  * $Date: 2003/06/12 18:06:11 $
00635  */
00636 
00637 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2