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
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
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
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
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
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
00117 worldStateMemRgn = InitRegion(sizeof(WorldState));
00118 state=new ((WorldState*)worldStateMemRgn->Base()) WorldState;
00119
00120
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);
00131
00132
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
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
00161
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
00222
00223
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
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
00253
00254
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
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
00279 motman->InitAccess(subject[sbjMotionManagerComm]);
00280 addRunLevel();
00281 }
00282 observer[obsReceiveMotionManager]->AssertReady();
00283 cout << "done" << endl;
00284 }
00285
00286
00287
00288
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
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
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)
00343 return;
00344
00345
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
00351
00352
00353
00354
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
00364
00365 bool isERS7;
00366 if(state!=NULL)
00367 isERS7=state->robotDesign&WorldState::ERS7Mask;
00368 else {
00369 char robotDesignStr[orobotdesignNAME_MAX];
00370 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00371 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00372 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00373 return;
00374 }
00375 isERS7=(strcmp(robotDesignStr,"ERS-7")==0);
00376 }
00377 if(ProcessID::getID()==ProcessID::MotionProcess) {
00378 float outputs[NumFrames][NumOutputs];
00379 if(state!=NULL) {
00380 motman->getOutputs(outputs);
00381 motman->updatePIDs(primIDs);
00382 motman->updateWorldState();
00383 } else {
00384 for(unsigned int f=0; f<NumFrames; f++)
00385 for(unsigned int i=0; i<NumOutputs; i++)
00386 outputs[f][i]=0;
00387 }
00388
00389
00390 unsigned int used=0;
00391 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00392 if(open[i]) {
00393 OJointCommandValue2* jval = reinterpret_cast<OJointCommandValue2*>(cmdVecData->GetData(used)->value);
00394 for(unsigned int frame=0; frame<NumFrames; frame++)
00395 jval[frame].value = (slongword)(outputs[frame][i]*1e6);
00396 used++;
00397 }
00398 if(isERS7) {
00399
00400 for(unsigned int i=LEDOffset; i<ERS7Info::FaceLEDPanelOffset; i++)
00401 if(open[i]) {
00402 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00403 for(unsigned int frame=0; frame<NumFrames; frame++)
00404 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00405 used++;
00406 }
00407
00408 OLED3Mode curMode[NumFrames];
00409 for(unsigned int frame=0; frame<NumFrames; frame++)
00410 curMode[frame]=(calcLEDValue(ERS7Info::LEDABModeOffset-LEDOffset,sqrt(clipRange01(outputs[frame][ERS7Info::LEDABModeOffset])))==oledON?oled3_MODE_B:oled3_MODE_A);
00411 for(unsigned int i=ERS7Info::FaceLEDPanelOffset; i<LEDOffset+NumLEDs; i++)
00412 if(open[i]) {
00413 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00414 for(unsigned int frame=0; frame<NumFrames; frame++) {
00415 jval[frame].intensity = static_cast<sword>(255*clipRange01(outputs[frame][i]));
00416 jval[frame].mode=curMode[frame];
00417 }
00418 used++;
00419 }
00420 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00421 if(open[i]) {
00422 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00423 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00424 jval[frame].value = (outputs[frame][i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00425 used++;
00426 }
00427 } else {
00428 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00429 if(open[i]) {
00430 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00431 for(unsigned int frame=0; frame<NumFrames; frame++)
00432 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00433 used++;
00434 }
00435 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00436 if(open[i]) {
00437 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00438 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00439 jval[frame].value = (outputs[frame][i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00440 used++;
00441 }
00442 }
00443 } else if(ProcessID::getID()==ProcessID::MainProcess) {
00444
00445 unsigned int used=0;
00446 if(isERS7) {
00447 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00448 if(open[i]) {
00449 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00450 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00451 jval[frame].value = (state->outputs[i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00452 used++;
00453 }
00454 } else {
00455 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00456 if(open[i]) {
00457 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00458 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00459 jval[frame].value = (state->outputs[i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00460 used++;
00461 }
00462 }
00463 }
00464
00465
00466 subject[sbjMoveJoint]->SetData(rgn);
00467
00468
00469
00470
00471 static unsigned int initCount=1;
00472 if(initCount<NUM_COMMAND_VECTOR) {
00473 initCount++;
00474 ReadySendJoints(sysevent);
00475 } else
00476 subject[sbjMoveJoint]->NotifyObservers();
00477 }
00478
00479 void
00480 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00481
00482
00483 if(isStopped) {
00484
00485 return;
00486 }
00487
00488 PROFSECTION("GotSensorFrame()",state->mainProfile);
00489 etrans.translateEvents();
00490
00491 OSensorFrameVectorData* rawsensor = reinterpret_cast<OSensorFrameVectorData*>(event.RCData(0)->Base());
00492 state->read(rawsensor[0],erouter);
00493 erouter->processTimers();
00494 static unsigned int throwaway=1;
00495 if(throwaway!=0) {
00496 throwaway--;
00497 if(throwaway==0)
00498 addRunLevel();
00499 }
00500 observer[obsSensorFrame]->AssertReady();
00501
00502
00503 }
00504
00505 void
00506 MMCombo::GotImage(const ONotifyEvent& event){
00507 if(isStopped) {
00508
00509 return;
00510 }
00511
00512 PROFSECTION("GotImage()",state->mainProfile);
00513 etrans.translateEvents();
00514
00515 erouter->processTimers();
00516
00517 WMvari(int, frame_counter, 0);
00518 ++frame_counter;
00519
00520 erouter->postEvent(new DataEvent<const OFbkImageVectorData*>(reinterpret_cast<const OFbkImageVectorData*>(event.Data(0)),EventBase::visOFbkEGID,0,EventBase::statusETID));
00521
00522 erouter->processTimers();
00523
00524 observer[obsImage]->AssertReady();
00525 }
00526
00527 void
00528 MMCombo::GotAudio(const ONotifyEvent& event){
00529 if(isStopped) {
00530
00531 return;
00532 }
00533
00534 PROFSECTION("GotAudio()",state->mainProfile);
00535 etrans.translateEvents();
00536
00537 erouter->postEvent(new DataEvent<const OSoundVectorData*>(reinterpret_cast<const OSoundVectorData*>(event.Data(0)),EventBase::micOSndEGID,0,EventBase::statusETID));
00538
00539 erouter->processTimers();
00540
00541 observer[obsMic]->AssertReady();
00542 }
00543
00544 void
00545 MMCombo::GotPowerEvent(void * msg){
00546 if(isStopped) {
00547
00548 return;
00549 }
00550
00551
00552 PROFSECTION("PowerEvent()",state->mainProfile);
00553 etrans.translateEvents();
00554
00555 static bool first=true;
00556 if(first) {
00557 addRunLevel();
00558 first=false;
00559 }
00560 const OPowerStatus* result = &static_cast<OPowerStatusMessage*>(msg)->powerStatus;
00561 state->read(*result,erouter);
00562 erouter->processTimers();
00563
00564
00565
00566
00567 if(state->powerFlags[PowerSourceID::PauseSID]) {
00568 cout << "%%%%%%% Pause button was pushed! %%%%%%%" << endl;
00569 OBootCondition bc(0);
00570 OPENR::Shutdown(bc);
00571 }
00572
00573 }
00574
00575 void
00576 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00577 if(isStopped) {
00578
00579 return;
00580 }
00581
00582
00583 if(motman!=NULL)
00584 motman->receivedMsg(event);
00585 else
00586 cout << "*** WARNING Main dropping MotionCommand (motman not ready) " << endl;
00587 observer[obsMotionManagerComm]->AssertReady();
00588
00589 }
00590
00591 void
00592 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00593 cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00594
00595 ASSERT(event.NumOfData()==1,"Too many SoundManagers");
00596 soundManagerMemRgn = event.RCData(0);
00597 soundManagerMemRgn->AddReference();
00598 sndman = reinterpret_cast<SoundManager*>(soundManagerMemRgn->Base());
00599 observer[obsReceiveSoundManager]->AssertReady();
00600 sndman->InitAccess(subject[sbjSoundManagerComm]);
00601 addRunLevel();
00602 cout << "done" << endl;
00603 }
00604
00605 void
00606 MMCombo::OpenPrimitives()
00607 {
00608 for(unsigned int i=0; i<NumOutputs; i++)
00609 if(open[i]) {
00610 OStatus result = OPENR::OpenPrimitive(PrimitiveName[i], &primIDs[i]);
00611 if (result != oSUCCESS)
00612 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoInit()","OPENR::OpenPrimitive() FAILED", result));
00613 }
00614 }
00615
00616 void
00617 MMCombo::SetupOutputs(const bool to_open[NumOutputs])
00618 {
00619 char robotDesignStr[orobotdesignNAME_MAX];
00620 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00621 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00622 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00623 return;
00624 } else {
00625 if(strcmp(robotDesignStr,"ERS-210")==0) {
00626 for(unsigned int j=0; j<NumOutputs; j++)
00627 open[j]=to_open[j] && ERS210Info::IsRealERS210[j];
00628 } else if(strcmp(robotDesignStr,"ERS-220")==0) {
00629 for(unsigned int j=0; j<NumOutputs; j++)
00630 open[j]=to_open[j] && ERS220Info::IsRealERS220[j];
00631 } else if(strcmp(robotDesignStr,"ERS-7")==0) {
00632 for(unsigned int j=0; j<NumOutputs; j++)
00633 open[j]=to_open[j] && ERS7Info::IsRealERS7[j];
00634 } else {
00635 cout << "MMCombo::SetupOutputs - ERROR: Unrecognized model: "<<robotDesignStr<<"\nSorry..."<<endl;
00636 return;
00637 }
00638 }
00639
00640
00641 for(unsigned int j=0; j<NumOutputs; j++)
00642 if(open[j])
00643 num_open++;
00644
00645 if(num_open==0)
00646 return;
00647
00648 OpenPrimitives();
00649
00650
00651 for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00652 MemoryRegionID cmdVecDataID;
00653 OCommandVectorData* cmdVecData;
00654 OStatus result = OPENR::NewCommandVectorData(num_open,&cmdVecDataID,&cmdVecData);
00655 if (result != oSUCCESS)
00656 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::NewCommandVectorData()","OPENR::NewCommandVectorData() FAILED", result));
00657 region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,cmdVecData->vectorInfo.offset,(void*)cmdVecData,cmdVecData->vectorInfo.totalSize);
00658 cmdVecData->SetNumData(num_open);
00659
00660
00661 unsigned int used=0;
00662 ASSERT(cmdVecData==reinterpret_cast<OCommandVectorData*>(region[i]->Base())," should be equal!?");
00663 for(unsigned int j=PIDJointOffset; j<PIDJointOffset+NumPIDJoints; j++)
00664 if(open[j]) {
00665 OCommandInfo* info = cmdVecData->GetInfo(used++);
00666 info->Set(odataJOINT_COMMAND2, primIDs[j], NumFrames);
00667 }
00668 if(strcmp(robotDesignStr,"ERS-7")==0) {
00669
00670 for(unsigned int j=LEDOffset; j<ERS7Info::FaceLEDPanelOffset; j++)
00671 if(open[j]) {
00672 OCommandInfo* info = cmdVecData->GetInfo(used);
00673 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00674 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00675 for(unsigned int frame=0; frame<NumFrames; frame++)
00676 jval[frame].period = 1;
00677 used++;
00678 }
00679
00680 for(unsigned int j=ERS7Info::FaceLEDPanelOffset; j<LEDOffset+NumLEDs; j++)
00681 if(open[j]) {
00682 OCommandInfo* info = cmdVecData->GetInfo(used);
00683 info->Set(odataLED_COMMAND3, primIDs[j], NumFrames);
00684 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00685 for(unsigned int frame=0; frame<NumFrames; frame++)
00686 jval[frame].period = 1;
00687 used++;
00688 }
00689
00690 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00691 if(open[j]) {
00692 OCommandInfo* info = cmdVecData->GetInfo(used);
00693 info->Set(odataJOINT_COMMAND4, primIDs[j], NumSlowFrames);
00694 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00695 for(unsigned int frame=0; frame<NumFrames; frame++)
00696 jval[frame].period = 1;
00697 used++;
00698 }
00699 } else {
00700 for(unsigned int j=LEDOffset; j<LEDOffset+NumLEDs; j++)
00701 if(open[j]) {
00702 OCommandInfo* info = cmdVecData->GetInfo(used);
00703 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00704 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00705 for(unsigned int frame=0; frame<NumFrames; frame++)
00706 jval[frame].period = 1;
00707 used++;
00708 }
00709 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00710 if(open[j]) {
00711 OCommandInfo* info = cmdVecData->GetInfo(used);
00712 info->Set(odataJOINT_COMMAND3, primIDs[j], NumSlowFrames);
00713 used++;
00714 }
00715 }
00716 }
00717 }
00718
00719
00720 RCRegion*
00721 MMCombo::InitRegion(unsigned int size) {
00722 unsigned int pagesize=4096;
00723 sError err=GetPageSize(&pagesize);
00724 ASSERT(err==sSUCCESS,"Error "<<err<<" getting page size");
00725 unsigned int pages=(size+pagesize-1)/pagesize;
00726 return new RCRegion(pages*pagesize);
00727 }
00728
00729 void
00730 MMCombo::addRunLevel() {
00731 runLevel++;
00732 if(runLevel==readyLevel) {
00733 cout << "START UP BEHAVIOR..." << flush;
00734 ProjectInterface::startupBehavior.DoStart();
00735 cout << "START UP BEHAVIOR-DONE" << endl;
00736 }
00737 }
00738
00739 void
00740 MMCombo::RPOPENR_notify(const ONotifyEvent& event) {
00741 const char *buf = (const char *)event.Data(0);
00742 observer[event.ObsIndex()]->AssertReady();
00743
00744
00745
00746
00747
00748
00749
00750
00751 erouter->postEvent(new TextMsgEvent(buf));
00752 }
00753
00754 int
00755 MMCombo::RPOPENR_send(char *buf, int bufsize) {
00756 if (RPOPENR_isready && bufsize>0) {
00757 RPOPENR_isready=false;
00758 subject[sbjRPOPENRSendString]->SetData(buf, bufsize);
00759 subject[sbjRPOPENRSendString]->NotifyObservers();
00760 return bufsize;
00761 }
00762 return 0;
00763 }
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776