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 + 1];
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 on 210, motion does everything else)
00391     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00392       if(open[i]) {
00393         float cal=config->motion.calibration[i-PIDJointOffset];
00394         OJointCommandValue2* jval = reinterpret_cast<OJointCommandValue2*>(cmdVecData->GetData(used)->value);
00395         for(unsigned int frame=0; frame<NumFrames; frame++)
00396           jval[frame].value = (slongword)(outputs[frame][i]*1e6*cal);
00397         used++;
00398       }
00399     if(isERS7) {
00400       // except if it's an ERS-7, we have to use different data structures for some of the leds and the ears
00401       for(unsigned int i=LEDOffset; i<ERS7Info::FaceLEDPanelOffset; i++)
00402         if(open[i]) {
00403           OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00404           for(unsigned int frame=0; frame<NumFrames; frame++)
00405             jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00406           used++;
00407         }
00408       // for instance, this virtual mode thing, which is global to all the affected LEDs
00409       OLED3Mode curMode[NumFrames];
00410       for(unsigned int frame=0; frame<NumFrames; frame++)
00411         curMode[frame]=(calcLEDValue(ERS7Info::LEDABModeOffset-LEDOffset,sqrt(clipRange01(outputs[frame][ERS7Info::LEDABModeOffset])))==oledON?oled3_MODE_B:oled3_MODE_A);
00412       for(unsigned int i=ERS7Info::FaceLEDPanelOffset; i<LEDOffset+NumLEDs; i++)
00413         if(open[i]) {
00414           OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00415           for(unsigned int frame=0; frame<NumFrames; frame++) {
00416             jval[frame].intensity = static_cast<sword>(255*clipRange01(outputs[frame][i]));
00417             jval[frame].mode=curMode[frame];
00418           }
00419           used++;
00420         }
00421       for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00422         if(open[i]) {
00423           OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00424           for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00425             jval[frame].value = (outputs[frame][i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00426           used++;
00427         }
00428     } else {
00429       for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00430         if(open[i]) {
00431           OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00432           for(unsigned int frame=0; frame<NumFrames; frame++)
00433             jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00434           used++;
00435         }
00436       for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00437         if(open[i]) {
00438           OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00439           for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00440             jval[frame].value = (outputs[frame][i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00441           used++;
00442         }
00443     }
00444   } else if(ProcessID::getID()==ProcessID::MainProcess) {
00445     // Just copy over the current ear state from WorldState
00446     unsigned int used=0; //but only copy open joints (so main does ears, motion does everything else)
00447     if(isERS7) {
00448       for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00449         if(open[i]) {
00450           OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00451           for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00452             jval[frame].value = (state->outputs[i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00453           used++;
00454         }
00455     } else {
00456       for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00457         if(open[i]) {
00458           OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00459           for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00460             jval[frame].value = (state->outputs[i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00461           used++;
00462         }
00463     }
00464   }
00465 
00466   // Send outputs to system
00467   subject[sbjMoveJoint]->SetData(rgn);
00468 
00469   // The first time this is called, we actually need to send *two* buffers
00470   // in order to get the double buffering going... (well, actually generalized
00471   // for NUM_COMMAND_VECTOR level buffering)
00472   static unsigned int initCount=1;
00473   if(initCount<NUM_COMMAND_VECTOR) {
00474     initCount++;
00475     ReadySendJoints(sysevent);
00476   } else //recursive base case
00477     subject[sbjMoveJoint]->NotifyObservers();
00478 }
00479 
00480 void
00481 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00482   //  if(state && state->buttons[RFrPawOffset])
00483   //  cout << "SENSOR..."<<flush;
00484   if(isStopped) {
00485     //cout << "BAH!GotSensorFrame" << endl;
00486     return;
00487   }
00488 
00489   PROFSECTION("GotSensorFrame()",state->mainProfile);
00490   etrans.translateEvents();
00491 
00492   OSensorFrameVectorData* rawsensor = reinterpret_cast<OSensorFrameVectorData*>(event.RCData(0)->Base());
00493   state->read(rawsensor[0],erouter);
00494   erouter->processTimers();
00495   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.
00496   if(throwaway!=0) {
00497     throwaway--;
00498     if(throwaway==0)
00499       addRunLevel();
00500   }
00501   observer[obsSensorFrame]->AssertReady();
00502   //  if(state && state->buttons[RFrPawOffset])
00503   //  cout << "done" << endl;
00504 }
00505 
00506 void
00507 MMCombo::GotImage(const ONotifyEvent& event){
00508   if(isStopped) {
00509     //cout << "BAH!GotImage" << endl;
00510     return;
00511   }
00512 
00513   PROFSECTION("GotImage()",state->mainProfile);
00514   etrans.translateEvents();
00515 
00516   erouter->processTimers();
00517   
00518   WMvari(int, frame_counter, 0);
00519   ++frame_counter;
00520   
00521   erouter->postEvent(new DataEvent<const OFbkImageVectorData*>(reinterpret_cast<const OFbkImageVectorData*>(event.Data(0)),EventBase::visOFbkEGID,0,EventBase::statusETID));
00522   
00523   erouter->processTimers();
00524   
00525   observer[obsImage]->AssertReady();
00526 }
00527 
00528 void
00529 MMCombo::GotAudio(const ONotifyEvent& event){
00530   if(isStopped) {
00531     //cout << "BAH!GotAudio" << endl;
00532     return;
00533   }
00534 
00535   PROFSECTION("GotAudio()",state->mainProfile);
00536   etrans.translateEvents();
00537 
00538   for (int i = 0; i < event.NumOfData(); i++) {
00539     erouter->postEvent(new DataEvent<const OSoundVectorData*>(reinterpret_cast<const OSoundVectorData*>(event.Data(i)),EventBase::micOSndEGID,0,EventBase::statusETID));
00540     erouter->processTimers();
00541   }
00542   
00543   observer[obsMic]->AssertReady();
00544 }
00545 
00546 void
00547 MMCombo::GotPowerEvent(void * msg){
00548   if(isStopped) {
00549     //cout << "BAH!GotPowerEvent" << endl;
00550     return;
00551   }
00552 
00553   //  cout << "POWER..."<<flush;
00554   PROFSECTION("PowerEvent()",state->mainProfile);
00555   etrans.translateEvents();
00556 
00557   static bool first=true;
00558   if(first) {
00559     addRunLevel();
00560     first=false;
00561   }
00562   const OPowerStatus* result = &static_cast<OPowerStatusMessage*>(msg)->powerStatus;
00563   state->read(*result,erouter);
00564   erouter->processTimers();
00565   // this part watches to see if the power button is pressed to shutdown the robot
00566   // i'm leaving this low-level because there's not much else you can do anyway...
00567   // the hardware kills power to the motors, and as far as we can tell, you can't
00568   // turn them back on.
00569   if(state->powerFlags[PowerSourceID::PauseSID]) {
00570     cout << "%%%%%%%  Pause button was pushed! %%%%%%%" << endl;
00571     OBootCondition bc(0);
00572     OPENR::Shutdown(bc);
00573   }
00574   //  cout << "done" << endl;
00575 }
00576 
00577 void
00578 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00579   if(isStopped) {
00580     //cout << "BAH!GotMotionMsg" << endl;
00581     return;
00582   }
00583 
00584   //  cout << "RECEIVE..."<<flush;
00585   if(motman!=NULL)
00586     motman->receivedMsg(event);
00587   else
00588     cout << "*** WARNING Main dropping MotionCommand (motman not ready) " << endl;
00589   observer[obsMotionManagerComm]->AssertReady();
00590   //  cout << "done" << endl;
00591 }
00592 
00593 void
00594 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00595   cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00596   //  PROFSECTION("GotMemRegion()",state->mainProfile);
00597   ASSERT(event.NumOfData()==1,"Too many SoundManagers");
00598   soundManagerMemRgn = event.RCData(0);
00599   soundManagerMemRgn->AddReference();
00600   sndman = reinterpret_cast<SoundManager*>(soundManagerMemRgn->Base());
00601   observer[obsReceiveSoundManager]->AssertReady();
00602   sndman->InitAccess(subject[sbjSoundManagerComm]);
00603   addRunLevel();
00604   cout << "done" << endl;
00605 }
00606 
00607 void
00608 MMCombo::OpenPrimitives()
00609 {
00610   for(unsigned int i=0; i<NumOutputs; i++)
00611     if(open[i]) {
00612       OStatus result = OPENR::OpenPrimitive(PrimitiveName[i], &primIDs[i]);
00613       if (result != oSUCCESS)
00614         OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoInit()","OPENR::OpenPrimitive() FAILED", result));
00615     }
00616 }
00617 
00618 void
00619 MMCombo::SetupOutputs(const bool to_open[NumOutputs])
00620 {
00621   char robotDesignStr[orobotdesignNAME_MAX + 1];
00622   memset(robotDesignStr, 0, sizeof(robotDesignStr));
00623   if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00624     cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00625     return;
00626   } else {
00627     if(strcmp(robotDesignStr,"ERS-210")==0) {
00628       for(unsigned int j=0; j<NumOutputs; j++)
00629         open[j]=to_open[j] && ERS210Info::IsRealERS210[j];
00630     } else if(strcmp(robotDesignStr,"ERS-220")==0) {
00631       for(unsigned int j=0; j<NumOutputs; j++)
00632         open[j]=to_open[j] && ERS220Info::IsRealERS220[j];
00633     } else if(strcmp(robotDesignStr,"ERS-7")==0) {
00634       for(unsigned int j=0; j<NumOutputs; j++)
00635         open[j]=to_open[j] && ERS7Info::IsRealERS7[j];
00636     } else {
00637       cout << "MMCombo::SetupOutputs - ERROR: Unrecognized model: "<<robotDesignStr<<"\nSorry..."<<endl;
00638       return;
00639     }
00640   }
00641   
00642   // count how many we're opening
00643   for(unsigned int j=0; j<NumOutputs; j++)
00644     if(open[j])
00645       num_open++;
00646 
00647   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)
00648     return;
00649 
00650   OpenPrimitives();
00651 
00652   // request memory regions
00653   for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00654     MemoryRegionID      cmdVecDataID;
00655     OCommandVectorData* cmdVecData;
00656     OStatus result = OPENR::NewCommandVectorData(num_open,&cmdVecDataID,&cmdVecData);
00657     if (result != oSUCCESS)
00658       OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::NewCommandVectorData()","OPENR::NewCommandVectorData() FAILED", result));
00659     region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,cmdVecData->vectorInfo.offset,(void*)cmdVecData,cmdVecData->vectorInfo.totalSize);
00660     cmdVecData->SetNumData(num_open);
00661 
00662     // initialize the outputs we just opened
00663     unsigned int used=0;
00664     ASSERT(cmdVecData==reinterpret_cast<OCommandVectorData*>(region[i]->Base())," should be equal!?");
00665     for(unsigned int j=PIDJointOffset; j<PIDJointOffset+NumPIDJoints; j++)
00666       if(open[j]) {
00667         OCommandInfo* info = cmdVecData->GetInfo(used++);
00668         info->Set(odataJOINT_COMMAND2, primIDs[j], NumFrames);
00669       }
00670     if(strcmp(robotDesignStr,"ERS-7")==0) {
00671       // this part's the same as usual, except stop when we get to face leds
00672       for(unsigned int j=LEDOffset; j<ERS7Info::FaceLEDPanelOffset; j++)
00673         if(open[j]) {
00674           OCommandInfo* info = cmdVecData->GetInfo(used);
00675           info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00676           OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00677           for(unsigned int frame=0; frame<NumFrames; frame++)
00678             jval[frame].period = 1;
00679           used++;
00680         }
00681       //we have to use OLEDCommandValue3 on the face and back LEDs if it's an ERS-7
00682       for(unsigned int j=ERS7Info::FaceLEDPanelOffset; j<LEDOffset+NumLEDs; j++)
00683         if(open[j]) {
00684           OCommandInfo* info = cmdVecData->GetInfo(used);
00685           info->Set(odataLED_COMMAND3, primIDs[j], NumFrames);
00686           OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00687           for(unsigned int frame=0; frame<NumFrames; frame++)
00688             jval[frame].period = 1;
00689           used++;
00690         }
00691       //also have to use OJointCommandValue4 on the ears now
00692       for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00693         if(open[j]) {
00694           OCommandInfo* info = cmdVecData->GetInfo(used);
00695           info->Set(odataJOINT_COMMAND4, primIDs[j], NumSlowFrames);
00696           OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00697           for(unsigned int frame=0; frame<NumFrames; frame++)
00698             jval[frame].period = 1;
00699           used++;
00700         }
00701     } else {
00702       for(unsigned int j=LEDOffset; j<LEDOffset+NumLEDs; j++)
00703         if(open[j]) {
00704           OCommandInfo* info = cmdVecData->GetInfo(used);
00705           info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00706           OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00707           for(unsigned int frame=0; frame<NumFrames; frame++)
00708             jval[frame].period = 1;
00709           used++;
00710         }
00711       for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00712         if(open[j]) {
00713           OCommandInfo* info = cmdVecData->GetInfo(used);
00714           info->Set(odataJOINT_COMMAND3, primIDs[j], NumSlowFrames);
00715           used++;
00716         }
00717     }
00718   }
00719 }
00720 
00721 /*! Will round up size to the nearest page */
00722 RCRegion*
00723 MMCombo::InitRegion(unsigned int size) {
00724   unsigned int pagesize=4096;
00725   sError err=GetPageSize(&pagesize);
00726   ASSERT(err==sSUCCESS,"Error "<<err<<" getting page size");
00727   unsigned int pages=(size+pagesize-1)/pagesize;
00728   return new RCRegion(pages*pagesize);
00729 }
00730 
00731 void
00732 MMCombo::addRunLevel() {
00733   runLevel++;
00734   if(runLevel==readyLevel) {
00735     cout << "START UP BEHAVIOR..." << flush;
00736     ProjectInterface::startupBehavior.DoStart();
00737     cout << "START UP BEHAVIOR-DONE" << endl;
00738   }
00739 }
00740 
00741 void
00742 MMCombo::RPOPENR_notify(const ONotifyEvent& event) {
00743   const char *buf = (const char *)event.Data(0);
00744   observer[event.ObsIndex()]->AssertReady();
00745 
00746   /* usercode: message received
00747    * an example could be- */
00748   // RPOPENR_send("yeah i received your message", 29);
00749 
00750   /* create TextMsg event which can contain strings
00751    * comment this out if you want to handle the received data exclusively
00752    */
00753   erouter->postEvent(new TextMsgEvent(buf));
00754 }
00755 
00756 int
00757 MMCombo::RPOPENR_send(char *buf, int bufsize) {
00758   if (RPOPENR_isready && bufsize>0) {
00759     RPOPENR_isready=false;
00760     subject[sbjRPOPENRSendString]->SetData(buf, bufsize);
00761     subject[sbjRPOPENRSendString]->NotifyObservers();
00762     return bufsize;
00763   }
00764   return 0;
00765 }
00766 
00767 /*! @file
00768  * @brief Implements MMCombo, the OObject which "forks" (sort of) into Main and Motion processes
00769  * @author ejt (Creator)
00770  *
00771  * $Author: ejt $
00772  * $Name: tekkotsu-2_2_1 $
00773  * $Revision: 1.60 $
00774  * $State: Exp $
00775  * $Date: 2004/11/12 22:07:39 $
00776  */
00777 
00778 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1