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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4