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