Homepage Demos Overview Downloads Tutorials Reference
Credits

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

Tekkotsu v2.1
Generated Tue Mar 16 23:19:14 2004 by Doxygen 1.3.5