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 MMCombo::MMCombo()
00030 : OObject(), motmanMemRgn(NULL), worldStateMemRgn(NULL),
00031 soundManagerMemRgn(NULL), eventTranslatorQueueMemRgn(NULL),
00032 runLevel(0), num_open(0), etrans(), RPOPENR_isready(true), isStopped(true)
00033 {
00034 for(unsigned int i=0; i<NumOutputs; i++) {
00035 primIDs[i]=oprimitiveID_UNDEF;
00036 open[i]=false;
00037 }
00038 Profiler::initBuckets();
00039 }
00040
00041
00042 OStatus
00043 MMCombo::DoInit(const OSystemEvent&)
00044 {
00045 cout << objectName << "::DoInit() " << endl;
00046
00047 isStopped=false;
00048
00049 NEW_ALL_SUBJECT_AND_OBSERVER;
00050 REGISTER_ALL_ENTRY;
00051 SET_ALL_READY_AND_NOTIFY_ENTRY;
00052
00053
00054 observer[obsReceiveWorldState]->SetBufCtrlParam(0,1,1);
00055 observer[obsReceiveMotionManager]->SetBufCtrlParam(0,1,1);
00056 observer[obsMotionManagerComm]->SetBufCtrlParam(0,1,MotionManager::MAX_MOTIONS+1);
00057
00058
00059 if(strcmp(objectName,"MainObj")==0)
00060 ProcessID::setID(ProcessID::MainProcess);
00061 else if(strcmp(objectName,"MotoObj")==0)
00062 ProcessID::setID(ProcessID::MotionProcess);
00063
00064
00065 config=new Config("/ms/config/tekkotsu.cfg");
00066
00067 erouter = new EventRouter;
00068
00069 if(strcmp(objectName,"MainObj")==0) {
00070 bool isSlowOutput[NumOutputs];
00071 for(unsigned int i=0; i<NumOutputs; i++)
00072 isSlowOutput[i]=!IsFastOutput[i];
00073
00074 SetupOutputs(isSlowOutput);
00075
00076
00077 OPowerStatus observationStatus;
00078 observationStatus.Set(orsbALL,obsbALL,opsoREMAINING_CAPACITY_NOTIFY_EVERY_CHANGE,opsoTEMPERATURE_NOTIFY_EVERY_CHANGE,opsoTIME_DIF_NOTIFY_EVERY_CHANGE,opsoVOLUME_NOTIFY_EVERY_CHANGE);
00079 OServiceEntry entry(myOID_, Extra_Entry[entryGotPowerEvent]);
00080 OStatus result = OPENR::ObservePowerStatus(observationStatus, entry);
00081 if(result != oSUCCESS) {
00082 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoStart()","OPENR::ObservePowerStatus() FAILED", result));
00083 return oFAIL;
00084 }
00085
00086
00087 wireless = new Wireless();
00088 sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6);
00089 serr=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*2);
00090 wireless->setDaemon(sout);
00091 wireless->setDaemon(serr);
00092 serr->setFlushType(SocketNS::FLUSH_BLOCKING);
00093 sout->setTextForward();
00094 serr->setForward(sout);
00095
00096
00097 worldStateMemRgn = InitRegion(sizeof(WorldState));
00098 state=new ((WorldState*)worldStateMemRgn->Base()) WorldState;
00099
00100
00101 eventTranslatorQueueMemRgn = InitRegion(sizeof(EventTranslator::Queue_t));
00102 EventTranslator::Queue_t * etransq=new ((EventTranslator::Queue_t*)eventTranslatorQueueMemRgn->Base()) EventTranslator::Queue_t;
00103 etrans.setQueue(etransq);
00104 MotionCommand::setQueue(etransq);
00105
00106 }
00107 if(strcmp(objectName,"MotoObj")==0) {
00108 SetupOutputs(IsFastOutput);
00109 OPENR::SetMotorPower(opowerON);
00110 OPENR::EnableJointGain(oprimitiveID_UNDEF);
00111
00112
00113 motmanMemRgn = InitRegion(sizeof(MotionManager));
00114 motman = new (motmanMemRgn->Base()) MotionManager();
00115 motman->InitAccess(subject[sbjMotionManagerComm]);
00116 }
00117 ProjectInterface::startupBehavior.AddReference();
00118
00119 cout << objectName << "::DoInit()-DONE" << endl;
00120 return oSUCCESS;
00121 }
00122
00123 OStatus
00124 MMCombo::DoStart(const OSystemEvent&)
00125 {
00126 cout << objectName << "::DoStart() " << endl;
00127
00128
00129
00130 if(strcmp(objectName,"MainObj")==0) {
00131 wireless->listen(sout, config->main.console_port);
00132 wireless->listen(serr, config->main.stderr_port);
00133 OPowerStatus power;
00134 OPENR::GetPowerStatus(&power);
00135 state->read(power,erouter);
00136 }
00137
00138 isStopped=false;
00139
00140 ENABLE_ALL_SUBJECT;
00141 ASSERT_READY_TO_ALL_OBSERVER;
00142
00143 addRunLevel();
00144
00145 cout << objectName << "::DoStart()-DONE" << endl;
00146 return oSUCCESS;
00147 }
00148
00149 OStatus
00150 MMCombo::DoStop(const OSystemEvent&)
00151 {
00152 cout << objectName << "::DoStop()..." << endl;
00153 if(strcmp(objectName,"MainObj")==0) {
00154 ProjectInterface::startupBehavior.DoStop();
00155 wireless->close(sout);
00156 wireless->close(serr);
00157 }
00158 DISABLE_ALL_SUBJECT;
00159 DEASSERT_READY_TO_ALL_OBSERVER;
00160 isStopped=true;
00161 cout << objectName << "::DoStop()-DONE" << endl;
00162 return oSUCCESS;
00163 }
00164
00165 OStatus
00166 MMCombo::DoDestroy(const OSystemEvent&)
00167 {
00168 cout << objectName << "::DoDestroy()..." << endl;
00169 ProjectInterface::startupBehavior.RemoveReference();
00170 if(strcmp(objectName,"MainObj")==0) {
00171 delete erouter;
00172 motmanMemRgn->RemoveReference();
00173 }
00174 if(strcmp(objectName,"MotoObj")==0) {
00175 worldStateMemRgn->RemoveReference();
00176 eventTranslatorQueueMemRgn->RemoveReference();
00177 }
00178 soundManagerMemRgn->RemoveReference();
00179 DELETE_ALL_SUBJECT_AND_OBSERVER;
00180 cout << objectName << "::DoDestroy()-DONE" << endl;
00181 return oSUCCESS;
00182 }
00183
00184
00185
00186
00187 void
00188 MMCombo::ReadyRegisterWorldState(const OReadyEvent&){
00189 static bool is_init=true;
00190 if(is_init) {
00191 is_init=false;
00192 cout << objectName << " Registering WorldState" << endl;
00193 if(strcmp(objectName,"MainObj")==0) {
00194 subject[sbjRegisterWorldState]->SetData(worldStateMemRgn);
00195 subject[sbjRegisterWorldState]->NotifyObservers();
00196 }
00197 }
00198 }
00199
00200 void
00201 MMCombo::GotWorldState(const ONotifyEvent& event){
00202 cout << objectName << "-GOTWORLDSTATE..." << flush;
00203
00204 if(strcmp(objectName,"MotoObj")==0) {
00205 ASSERT(event.NumOfData()==1,"Too many WorldStates");
00206 worldStateMemRgn = event.RCData(0);
00207 worldStateMemRgn->AddReference();
00208 state = reinterpret_cast<WorldState*>(worldStateMemRgn->Base());
00209 }
00210 observer[obsReceiveWorldState]->AssertReady();
00211 cout << "done" << endl;
00212 }
00213
00214
00215
00216
00217
00218 void
00219 MMCombo::ReadyRegisterMotionManager(const OReadyEvent&){
00220 static bool is_init=true;
00221 if(is_init) {
00222 is_init=false;
00223 cout << objectName << " Registering MotionManager" << endl;
00224 if(strcmp(objectName,"MotoObj")==0) {
00225 subject[sbjRegisterMotionManager]->SetData(motmanMemRgn);
00226 subject[sbjRegisterMotionManager]->NotifyObservers();
00227 }
00228 }
00229 }
00230
00231 void
00232 MMCombo::GotMotionManager(const ONotifyEvent& event){
00233 cout << objectName << "-GOTWORLDSTATE..." << flush;
00234
00235 if(strcmp(objectName,"MainObj")==0) {
00236 ASSERT(event.NumOfData()==1,"Too many MotionManagers");
00237 motmanMemRgn = event.RCData(0);
00238 motmanMemRgn->AddReference();
00239 motman = reinterpret_cast<MotionManager*>(motmanMemRgn->Base());
00240 cout << "MAIN INIT MOTMAN..." << flush;
00241
00242 motman->InitAccess(subject[sbjMotionManagerComm]);
00243 addRunLevel();
00244 }
00245 observer[obsReceiveMotionManager]->AssertReady();
00246 cout << "done" << endl;
00247 }
00248
00249
00250
00251
00252 void
00253 MMCombo::ReadyRegisterEventTranslatorQueue(const OReadyEvent&){
00254 static bool is_init=true;
00255 if(is_init) {
00256 is_init=false;
00257 cout << objectName << " Registering EventTranslatorQueue" << endl;
00258 if(strcmp(objectName,"MainObj")==0) {
00259 subject[sbjRegisterEventTranslatorQueue]->SetData(eventTranslatorQueueMemRgn);
00260 subject[sbjRegisterEventTranslatorQueue]->NotifyObservers();
00261 }
00262 }
00263 }
00264
00265 void
00266 MMCombo::GotEventTranslatorQueue(const ONotifyEvent& event){
00267 cout << objectName << "-GOTEventTranslatorQueue..." << flush;
00268
00269 if(strcmp(objectName,"MotoObj")==0) {
00270 ASSERT(event.NumOfData()==1,"Too many EventTranslatorQueue");
00271 eventTranslatorQueueMemRgn = event.RCData(0);
00272 eventTranslatorQueueMemRgn->AddReference();
00273 EventTranslator::Queue_t * etransq=reinterpret_cast<EventTranslator::Queue_t*>(eventTranslatorQueueMemRgn->Base());
00274 etrans.setQueue(etransq);
00275 MotionCommand::setQueue(etransq);
00276 erouter->addTrapper(&etrans);
00277 }
00278 observer[obsReceiveEventTranslatorQueue]->AssertReady();
00279 cout << "done" << endl;
00280 }
00281
00282
00283 void
00284 MMCombo::ReadySendJoints(const OReadyEvent& sysevent) {
00285
00286 if(isStopped) {
00287
00288 return;
00289 }
00290
00291 static unsigned int id=-1U;
00292 Profiler::Timer timer;
00293 if(ProcessID::getID()==ProcessID::MotionProcess) {
00294 if(state) {
00295 if(id==-1U)
00296 id=state->motionProfile.getNewID("ReadySendJoints()");
00297 timer.setID(id,&state->motionProfile);
00298 }
00299 } else if(ProcessID::getID()==ProcessID::MainProcess) {
00300 if(id==-1U)
00301 id=state->mainProfile.getNewID("ReadySendJoints()");
00302 timer.setID(id,&state->mainProfile);
00303 }
00304
00305 if(num_open==0)
00306 return;
00307
00308
00309 RCRegion* rgn=NULL;
00310 for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00311 if (region[i]->NumberOfReference() == 1) {
00312 rgn=region[i];
00313
00314
00315
00316
00317
00318
00319 break;
00320 }
00321 }
00322 ASSERTRET(rgn!=NULL,"Could not find unused command vector");
00323 ASSERTRET(rgn->Base()!=NULL,"Bad Command Vector");
00324 OCommandVectorData* cmdVecData = reinterpret_cast<OCommandVectorData*>(rgn->Base());
00325
00326
00327
00328 bool isERS7;
00329 if(state!=NULL)
00330 isERS7=state->robotDesign&WorldState::ERS7Mask;
00331 else {
00332 char robotDesignStr[orobotdesignNAME_MAX];
00333 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00334 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00335 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00336 return;
00337 }
00338 isERS7=(strcmp(robotDesignStr,"ERS-7")==0);
00339 }
00340 if(ProcessID::getID()==ProcessID::MotionProcess) {
00341 float outputs[NumFrames][NumOutputs];
00342 if(state!=NULL) {
00343 motman->getOutputs(outputs);
00344 motman->updatePIDs(primIDs);
00345 motman->updateWorldState();
00346 } else {
00347 for(unsigned int f=0; f<NumFrames; f++)
00348 for(unsigned int i=0; i<NumOutputs; i++)
00349 outputs[f][i]=0;
00350 }
00351
00352
00353 unsigned int used=0;
00354 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00355 if(open[i]) {
00356 OJointCommandValue2* jval = reinterpret_cast<OJointCommandValue2*>(cmdVecData->GetData(used)->value);
00357 for(unsigned int frame=0; frame<NumFrames; frame++)
00358 jval[frame].value = (slongword)(outputs[frame][i]*1e6);
00359 used++;
00360 }
00361 if(isERS7) {
00362
00363 for(unsigned int i=LEDOffset; i<ERS7Info::FaceLEDPanelOffset; i++)
00364 if(open[i]) {
00365 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00366 for(unsigned int frame=0; frame<NumFrames; frame++)
00367 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00368 used++;
00369 }
00370
00371 OLED3Mode curMode[NumFrames];
00372 for(unsigned int frame=0; frame<NumFrames; frame++)
00373 curMode[frame]=(calcLEDValue(ERS7Info::LEDABModeOffset-LEDOffset,sqrt(clipRange01(outputs[frame][ERS7Info::LEDABModeOffset])))==oledON?oled3_MODE_B:oled3_MODE_A);
00374 for(unsigned int i=ERS7Info::FaceLEDPanelOffset; i<LEDOffset+NumLEDs; i++)
00375 if(open[i]) {
00376 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00377 for(unsigned int frame=0; frame<NumFrames; frame++) {
00378 jval[frame].intensity = static_cast<sword>(255*clipRange01(outputs[frame][i]));
00379 jval[frame].mode=curMode[frame];
00380 }
00381 used++;
00382 }
00383 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00384 if(open[i]) {
00385 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00386 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00387 jval[frame].value = (outputs[frame][i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00388 used++;
00389 }
00390 } else {
00391 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00392 if(open[i]) {
00393 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00394 for(unsigned int frame=0; frame<NumFrames; frame++)
00395 jval[frame].led = calcLEDValue(i-LEDOffset,outputs[frame][i]);
00396 used++;
00397 }
00398 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00399 if(open[i]) {
00400 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00401 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00402 jval[frame].value = (outputs[frame][i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00403 used++;
00404 }
00405 }
00406 } else if(ProcessID::getID()==ProcessID::MainProcess) {
00407
00408 unsigned int used=0;
00409 if(isERS7) {
00410 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00411 if(open[i]) {
00412 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00413 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00414 jval[frame].value = (state->outputs[i]<.5?ojoint4_STATE0:ojoint4_STATE1);
00415 used++;
00416 }
00417 } else {
00418 for(unsigned int i=BinJointOffset; i<BinJointOffset+NumBinJoints; i++)
00419 if(open[i]) {
00420 OJointCommandValue3* jval = reinterpret_cast<OJointCommandValue3*>(cmdVecData->GetData(used)->value);
00421 for(unsigned int frame=0; frame<NumSlowFrames; frame++)
00422 jval[frame].value = (state->outputs[i]<.5?ojoint3_STATE1:ojoint3_STATE0);
00423 used++;
00424 }
00425 }
00426 }
00427
00428
00429 subject[sbjMoveJoint]->SetData(rgn);
00430
00431
00432
00433
00434 static unsigned int initCount=1;
00435 if(initCount<NUM_COMMAND_VECTOR) {
00436 initCount++;
00437 ReadySendJoints(sysevent);
00438 } else
00439 subject[sbjMoveJoint]->NotifyObservers();
00440 }
00441
00442 void
00443 MMCombo::GotSensorFrame(const ONotifyEvent& event){
00444
00445
00446 if(isStopped) {
00447
00448 return;
00449 }
00450
00451 PROFSECTION("GotSensorFrame()",state->mainProfile);
00452 etrans.translateEvents();
00453
00454 OSensorFrameVectorData* rawsensor = reinterpret_cast<OSensorFrameVectorData*>(event.RCData(0)->Base());
00455 state->read(rawsensor[0],erouter);
00456 erouter->processTimers();
00457 static unsigned int throwaway=1;
00458 if(throwaway!=0) {
00459 throwaway--;
00460 if(throwaway==0)
00461 addRunLevel();
00462 }
00463 observer[obsSensorFrame]->AssertReady();
00464
00465
00466 }
00467
00468 void
00469 MMCombo::GotImage(const ONotifyEvent& event){
00470 if(isStopped) {
00471
00472 return;
00473 }
00474
00475 PROFSECTION("GotImage()",state->mainProfile);
00476 etrans.translateEvents();
00477
00478 WMvari(int, frame_counter, 0);
00479 ++frame_counter;
00480
00481 erouter->postEvent(new DataEvent<const OFbkImageVectorData*>(reinterpret_cast<const OFbkImageVectorData*>(event.Data(0)),EventBase::visOFbkEGID,0,EventBase::statusETID));
00482
00483 erouter->processTimers();
00484
00485 observer[obsImage]->AssertReady();
00486 }
00487
00488 void
00489 MMCombo::GotPowerEvent(void * msg){
00490 if(isStopped) {
00491
00492 return;
00493 }
00494
00495
00496 PROFSECTION("PowerEvent()",state->mainProfile);
00497 etrans.translateEvents();
00498
00499 static bool first=true;
00500 if(first) {
00501 addRunLevel();
00502 first=false;
00503 }
00504 const OPowerStatus* result = &static_cast<OPowerStatusMessage*>(msg)->powerStatus;
00505 state->read(*result,erouter);
00506 erouter->processTimers();
00507
00508
00509
00510
00511 if(state->powerFlags[PowerSourceID::PauseSID]) {
00512 cout << "%%%%%%% Pause button was pushed! %%%%%%%" << endl;
00513 OBootCondition bc(0);
00514 OPENR::Shutdown(bc);
00515 }
00516
00517 }
00518
00519 void
00520 MMCombo::GotMotionMsg(const ONotifyEvent& event){
00521 if(isStopped) {
00522
00523 return;
00524 }
00525
00526
00527 if(motman!=NULL)
00528 motman->receivedMsg(event);
00529 else
00530 cout << "*** WARNING Main dropping MotionCommand (motman not ready) " << endl;
00531 observer[obsMotionManagerComm]->AssertReady();
00532
00533 }
00534
00535 void
00536 MMCombo::GotSoundManager(const ONotifyEvent& event) {
00537 cout << objectName << "-GOTSOUNDMANAGER..." << flush;
00538
00539 ASSERT(event.NumOfData()==1,"Too many SoundManagers");
00540 soundManagerMemRgn = event.RCData(0);
00541 soundManagerMemRgn->AddReference();
00542 sndman = reinterpret_cast<SoundManager*>(soundManagerMemRgn->Base());
00543 observer[obsReceiveSoundManager]->AssertReady();
00544 sndman->InitAccess(subject[sbjSoundManagerComm]);
00545 addRunLevel();
00546 cout << "done" << endl;
00547 }
00548
00549 void
00550 MMCombo::OpenPrimitives()
00551 {
00552 for(unsigned int i=0; i<NumOutputs; i++)
00553 if(open[i]) {
00554 OStatus result = OPENR::OpenPrimitive(PrimitiveName[i], &primIDs[i]);
00555 if (result != oSUCCESS)
00556 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoInit()","OPENR::OpenPrimitive() FAILED", result));
00557 }
00558 }
00559
00560 void
00561 MMCombo::SetupOutputs(const bool to_open[NumOutputs])
00562 {
00563 char robotDesignStr[orobotdesignNAME_MAX];
00564 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00565 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00566 cout << objectName << "::SetupOutputs - OPENR::GetRobotDesign() failed." << endl;
00567 return;
00568 } else {
00569 if(strcmp(robotDesignStr,"ERS-210")==0) {
00570 for(unsigned int j=0; j<NumOutputs; j++)
00571 open[j]=to_open[j] && ERS210Info::IsRealERS210[j];
00572 } else if(strcmp(robotDesignStr,"ERS-220")==0) {
00573 for(unsigned int j=0; j<NumOutputs; j++)
00574 open[j]=to_open[j] && ERS220Info::IsRealERS220[j];
00575 } else if(strcmp(robotDesignStr,"ERS-7")==0) {
00576 for(unsigned int j=0; j<NumOutputs; j++)
00577 open[j]=to_open[j] && ERS7Info::IsRealERS7[j];
00578 } else {
00579 cout << "MMCombo::SetupOutputs - ERROR: Unrecognized model: "<<robotDesignStr<<"\nSorry..."<<endl;
00580 return;
00581 }
00582 }
00583
00584
00585 for(unsigned int j=0; j<NumOutputs; j++)
00586 if(open[j])
00587 num_open++;
00588
00589 if(num_open==0)
00590 return;
00591
00592 OpenPrimitives();
00593
00594
00595 for (unsigned int i = 0; i < NUM_COMMAND_VECTOR; i++) {
00596 MemoryRegionID cmdVecDataID;
00597 OCommandVectorData* cmdVecData;
00598 OStatus result = OPENR::NewCommandVectorData(num_open,&cmdVecDataID,&cmdVecData);
00599 if (result != oSUCCESS)
00600 OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::NewCommandVectorData()","OPENR::NewCommandVectorData() FAILED", result));
00601 region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,cmdVecData->vectorInfo.offset,(void*)cmdVecData,cmdVecData->vectorInfo.totalSize);
00602 cmdVecData->SetNumData(num_open);
00603
00604
00605 unsigned int used=0;
00606 ASSERT(cmdVecData==reinterpret_cast<OCommandVectorData*>(region[i]->Base())," should be equal!?");
00607 for(unsigned int j=PIDJointOffset; j<PIDJointOffset+NumPIDJoints; j++)
00608 if(open[j]) {
00609 OCommandInfo* info = cmdVecData->GetInfo(used++);
00610 info->Set(odataJOINT_COMMAND2, primIDs[j], NumFrames);
00611 }
00612 if(strcmp(robotDesignStr,"ERS-7")==0) {
00613
00614 for(unsigned int j=LEDOffset; j<ERS7Info::FaceLEDPanelOffset; j++)
00615 if(open[j]) {
00616 OCommandInfo* info = cmdVecData->GetInfo(used);
00617 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00618 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00619 for(unsigned int frame=0; frame<NumFrames; frame++)
00620 jval[frame].period = 1;
00621 used++;
00622 }
00623
00624 for(unsigned int j=ERS7Info::FaceLEDPanelOffset; j<LEDOffset+NumLEDs; j++)
00625 if(open[j]) {
00626 OCommandInfo* info = cmdVecData->GetInfo(used);
00627 info->Set(odataLED_COMMAND3, primIDs[j], NumFrames);
00628 OLEDCommandValue3* jval = reinterpret_cast<OLEDCommandValue3*>(cmdVecData->GetData(used)->value);
00629 for(unsigned int frame=0; frame<NumFrames; frame++)
00630 jval[frame].period = 1;
00631 used++;
00632 }
00633
00634 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00635 if(open[j]) {
00636 OCommandInfo* info = cmdVecData->GetInfo(used);
00637 info->Set(odataJOINT_COMMAND4, primIDs[j], NumSlowFrames);
00638 OJointCommandValue4* jval = reinterpret_cast<OJointCommandValue4*>(cmdVecData->GetData(used)->value);
00639 for(unsigned int frame=0; frame<NumFrames; frame++)
00640 jval[frame].period = 1;
00641 used++;
00642 }
00643 } else {
00644 for(unsigned int j=LEDOffset; j<LEDOffset+NumLEDs; j++)
00645 if(open[j]) {
00646 OCommandInfo* info = cmdVecData->GetInfo(used);
00647 info->Set(odataLED_COMMAND2, primIDs[j], NumFrames);
00648 OLEDCommandValue2* jval = reinterpret_cast<OLEDCommandValue2*>(cmdVecData->GetData(used)->value);
00649 for(unsigned int frame=0; frame<NumFrames; frame++)
00650 jval[frame].period = 1;
00651 used++;
00652 }
00653 for(unsigned int j=BinJointOffset; j<BinJointOffset+NumBinJoints; j++)
00654 if(open[j]) {
00655 OCommandInfo* info = cmdVecData->GetInfo(used);
00656 info->Set(odataJOINT_COMMAND3, primIDs[j], NumSlowFrames);
00657 used++;
00658 }
00659 }
00660 }
00661 }
00662
00663
00664 RCRegion*
00665 MMCombo::InitRegion(unsigned int size) {
00666 unsigned int pagesize=4096;
00667 sError err=GetPageSize(&pagesize);
00668 ASSERT(err==sSUCCESS,"Error "<<err<<" getting page size");
00669 unsigned int pages=(size+pagesize-1)/pagesize;
00670 return new RCRegion(pages*pagesize);
00671 }
00672
00673 void
00674 MMCombo::addRunLevel() {
00675 runLevel++;
00676 if(runLevel==readyLevel) {
00677 cout << "START UP BEHAVIOR..." << flush;
00678 ProjectInterface::startupBehavior.DoStart();
00679 cout << "START UP BEHAVIOR-DONE" << endl;
00680 }
00681 }
00682
00683 void
00684 MMCombo::RPOPENR_notify(const ONotifyEvent& event) {
00685 const char *buf = (const char *)event.Data(0);
00686 observer[event.ObsIndex()]->AssertReady();
00687
00688
00689
00690
00691
00692
00693
00694
00695 erouter->postEvent(new TextMsgEvent(buf));
00696 }
00697
00698 int
00699 MMCombo::RPOPENR_send(char *buf, int bufsize) {
00700 if (RPOPENR_isready && bufsize>0) {
00701 RPOPENR_isready=false;
00702 subject[sbjRPOPENRSendString]->SetData(buf, bufsize);
00703 subject[sbjRPOPENRSendString]->NotifyObservers();
00704 return bufsize;
00705 }
00706 return 0;
00707 }
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720