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
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;
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;
00040 OLED3Mode mode;
00041 word period;
00042 word reserved;
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
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
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
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
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
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
00114 worldStateMemRgn = InitRegion(sizeof(WorldState));
00115 state=new ((WorldState*)worldStateMemRgn->Base()) WorldState;
00116
00117
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);
00128
00129
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
00146
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
00202
00203
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
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
00233
00234
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
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
00259 motman->InitAccess(subject[sbjMotionManagerComm]);
00260 addRunLevel();
00261 }
00262 observer[obsReceiveMotionManager]->AssertReady();
00263 cout << "done" << endl;
00264 }
00265
00266
00267
00268
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
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
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)
00323 return;
00324
00325
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
00331
00332
00333
00334
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
00344
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
00370 unsigned int used=0;
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
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
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
00425 unsigned int used=0;
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
00446 subject[sbjMoveJoint]->SetData(rgn);
00447
00448
00449
00450
00451 static unsigned int initCount=1;
00452 if(initCount<NUM_COMMAND_VECTOR) {
00453 initCount++;
00454 ReadySendJoints(sysevent);
00455 } else
00456 subject[sbjMoveJoint]->NotifyObservers();
00457 }
00458
00459 void
00460 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00461
00462
00463 if(isStopped) {
00464
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;
00475 if(throwaway!=0) {
00476 throwaway--;
00477 if(throwaway==0)
00478 addRunLevel();
00479 }
00480 observer[obsSensorFrame]->AssertReady();
00481
00482
00483 }
00484
00485 void
00486 MMCombo::GotImage(const ONotifyEvent& event){
00487 if(isStopped) {
00488
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
00509 return;
00510 }
00511
00512
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
00525
00526
00527
00528 if(state->powerFlags[PowerSourceID::PauseSID]) {
00529 cout << "%%%%%%% Pause button was pushed! %%%%%%%" << endl;
00530 OBootCondition bc(0);
00531 OPENR::Shutdown(bc);
00532 }
00533
00534 }
00535
00536 void
00537 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00538 if(isStopped) {
00539
00540 return;
00541 }
00542
00543
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
00550 }
00551
00552 void
00553 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00554 cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00555
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
00602 for(unsigned int j=0; j<NumOutputs; j++)
00603 if(open[j])
00604 num_open++;
00605
00606 if(num_open==0)
00607 return;
00608
00609 OpenPrimitives();
00610
00611
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
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
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
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
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
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
00708
00709
00710
00711
00712
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
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739