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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1