MotionManager.ccGo to the documentation of this file.00001 #include "MotionManager.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "Events/EventRouter.h"
00005
00006 #include "Shared/ERS210Info.h"
00007 #include "Shared/ERS220Info.h"
00008 #include "Shared/ERS7Info.h"
00009
00010 #include <list>
00011
00012 MotionManager * motman=NULL;
00013 int MotionManager::_MMaccID=-1U;
00014 EventTranslator* MotionManager::etrans=NULL;
00015
00016 const float MotionManager::kIgnoredPriority =-1;
00017 const float MotionManager::kBackgroundPriority = 0;
00018 const float MotionManager::kLowPriority = 5;
00019 const float MotionManager::kStdPriority = 10;
00020 const float MotionManager::kHighPriority = 50;
00021 const float MotionManager::kEmergencyPriority = 100;
00022
00023 using namespace std;
00024
00025
00026 typedef unsigned int uint;
00027
00028 MotionManager::MotionManager()
00029 : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock(),numAcc(0)
00030 {
00031 for(uint x=0; x<NumOutputs; x++)
00032 cmdSums[x]=0;
00033 }
00034
00035 #ifdef PLATFORM_APERIOS
00036
00037 void
00038 MotionManager::InitAccess(OSubject* subj) {
00039 if(numAcc==MAX_ACCESS) {
00040 printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00041 return;
00042 }
00043 _MMaccID=numAcc++;
00044
00045
00046
00047 MMlock.lock(_MMaccID);
00048
00049 subjs[_MMaccID]=subj;
00050 if(cmdlist.size()>0)
00051 cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00052 MMlock.unlock();
00053 }
00054
00055 #else //now PLATFORM_LOCAL
00056
00057 void
00058 MotionManager::InitAccess(MessageQueueBase& mcbufq) {
00059 if(numAcc==MAX_ACCESS) {
00060 printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00061 return;
00062 }
00063 _MMaccID=numAcc++;
00064
00065
00066
00067 MMlock.lock(_MMaccID);
00068 subjs[_MMaccID]=&mcbufq;
00069 if(cmdlist.size()>0)
00070 cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00071 MMlock.unlock();
00072 }
00073
00074 void
00075 MotionManager::RemoveAccess() {
00076 func_begin();
00077 for(MC_ID mc_id=cmdlist.begin(); mc_id!=cmdlist.end(); mc_id=cmdlist.next(mc_id)) {
00078 if(cmdlist[mc_id].rcr[_MMaccID]!=NULL) {
00079 checkoutMotion(mc_id,true);
00080 cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00081 cmdlist[mc_id].rcr[_MMaccID]=NULL;
00082 bool found=false;
00083 for(unsigned int i=0; i<numAcc; i++)
00084 if(cmdlist[mc_id].rcr[i]!=NULL) {
00085 found=true;
00086 break;
00087 }
00088 if(!found) {
00089 cout << "Warning: dropping motion command " << mc_id << ", was active at shutdown (leaked?)" << endl;
00090 push_free(mc_id);
00091
00092
00093 } else
00094 checkinMotion(mc_id);
00095 }
00096 }
00097 func_end();
00098 }
00099
00100 #endif //PLATFORM-specific initialization
00101
00102 MotionManager::~MotionManager() {
00103 if(!cmdlist.empty()) {
00104 func_begin();
00105 cout << "WARNING: MotionManager destruction with MotionCommands still attached." << endl;
00106 while(!cmdlist.empty()) {
00107 MC_ID mc_id=cmdlist.begin();
00108 for(unsigned int i=0; i<numAcc; i++)
00109 if(cmdlist[mc_id].rcr[i]!=NULL)
00110 cout << "MC " << mc_id << " was still referenced by InitAccess caller #" << _MMaccID << endl;
00111 push_free(mc_id);
00112 }
00113 func_end();
00114 }
00115 }
00116
00117 void
00118 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
00119 if(output >= NumOutputs)
00120 return;
00121 if(cmd.weight<=0)
00122 return;
00123
00124 if(caller==NULL || caller->getID()!=cur_cmd)
00125 func_begin();
00126 if(cur_cmd==invalid_MC_ID) {
00127 cmdSums[output]=cmd.value;
00128 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00129 cmdstatelist_t& curstatelist=cmdstates[output];
00130 cmdstatelist_t::index_t ent=curstatelist.begin();
00131 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00132 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
00133 else
00134 for(unsigned int i=0; i<NumFrames; i++)
00135 curstatelist[ent].frames[i]=cmd;
00136 }
00137 if(caller==NULL || caller->getID()!=cur_cmd)
00138 func_end();
00139 }
00140
00141 void
00142 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
00143 if(output >= NumOutputs)
00144 return;
00145 if(cmd.weight<=0)
00146 return;
00147
00148 if(caller==NULL || caller->getID()!=cur_cmd)
00149 func_begin();
00150 if(cur_cmd==invalid_MC_ID) {
00151 cmdSums[output]=cmd.value;
00152 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00153 cmdstatelist_t& curstatelist=cmdstates[output];
00154 cmdstatelist_t::index_t ent=curstatelist.begin();
00155 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00156 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
00157 else
00158 curstatelist[ent].frames[frame]=cmd;
00159 }
00160 if(caller==NULL || caller->getID()!=cur_cmd)
00161 func_end();
00162 }
00163
00164 void
00165 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
00166 if(output >= NumOutputs)
00167 return;
00168 unsigned int hasWeight=NumFrames;
00169 for(unsigned int i=NumFrames-1; i!=0; i--)
00170 if(ocmds[i].weight>0) {
00171 hasWeight=i;
00172 break;
00173 }
00174 if(hasWeight==NumFrames)
00175 return;
00176
00177
00178 if(caller==NULL || caller->getID()!=cur_cmd)
00179 func_begin();
00180 if(cur_cmd==invalid_MC_ID) {
00181 cmdSums[output]=ocmds[hasWeight].value;
00182 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00183 cmdstatelist_t& curstatelist=cmdstates[output];
00184 cmdstatelist_t::index_t ent=curstatelist.begin();
00185 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00186 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
00187 else
00188 for(unsigned int i=0; i<NumFrames; i++)
00189 curstatelist[ent].frames[i]=ocmds[i];
00190 }
00191 if(caller==NULL || caller->getID()!=cur_cmd)
00192 func_end();
00193 }
00194
00195 void
00196 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
00197 if(output >= NumOutputs)
00198 return;
00199
00200 if(caller==NULL || caller->getID()!=cur_cmd)
00201 func_begin();
00202 if(cur_cmd==invalid_MC_ID) {
00203 setPID(output,pid.pid);
00204 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00205 cmdstatelist_t& curstatelist=cmdstates[output];
00206 cmdstatelist_t::index_t ent=curstatelist.begin();
00207 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00208 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
00209 else
00210 curstatelist[ent].pid=pid;
00211 }
00212 if(caller==NULL || caller->getID()!=cur_cmd)
00213 func_end();
00214 }
00215
00216 void
00217 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
00218 if(output >= NumOutputs)
00219 return;
00220
00221 if(caller==NULL || caller->getID()!=cur_cmd)
00222 func_begin();
00223 if(cur_cmd==invalid_MC_ID) {
00224 if(cmd.weight>0)
00225 cmdSums[output]=cmd.value;
00226 setPID(output,pid.pid);
00227 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00228 cmdstatelist_t& curstatelist=cmdstates[output];
00229 cmdstatelist_t::index_t ent=curstatelist.begin();
00230 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00231 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
00232 else {
00233 for(unsigned int i=0; i<NumFrames; i++)
00234 curstatelist[ent].frames[i]=cmd;
00235 curstatelist[ent].pid=pid;
00236 }
00237 }
00238 if(caller==NULL || caller->getID()!=cur_cmd)
00239 func_end();
00240 }
00241
00242 void
00243 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
00244 if(output >= NumOutputs)
00245 return;
00246
00247 if(caller==NULL || caller->getID()!=cur_cmd)
00248 func_begin();
00249 if(cur_cmd==invalid_MC_ID) {
00250 if(ocmds[NumFrames-1].weight>0)
00251 cmdSums[output]=ocmds[NumFrames-1].value;
00252 setPID(output,pid.pid);
00253 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00254 cmdstatelist_t& curstatelist=cmdstates[output];
00255 cmdstatelist_t::index_t ent=curstatelist.begin();
00256 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00257 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds,pid));
00258 else {
00259 for(unsigned int i=0; i<NumFrames; i++)
00260 curstatelist[ent].frames[i]=ocmds[i];
00261 curstatelist[ent].pid=pid;
00262 }
00263 }
00264 if(caller==NULL || caller->getID()!=cur_cmd)
00265 func_end();
00266 }
00267
00268
00269
00270
00271 void
00272 MotionManager::getOutputs(float outputs[NumFrames][NumOutputs]) {
00273
00274
00275 if(state==NULL) {
00276
00277 for(uint f=0;f<NumFrames;f++)
00278 for(uint i=0; i<NumOutputs; i++)
00279 outputs[f][i]=0;
00280 for(uint f=0;f<NumFrames;f++)
00281 for(uint l=0; l<NumLEDs; l++)
00282 outputs[f][l]=l/(NumLEDs-1.0);
00283
00284
00285 return;
00286 }
00287 func_begin();
00288
00289
00290
00291 for(uint output=0; output<NumOutputs; output++)
00292 cmdstates[output].clear();
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305 for(uint output=0; output<NumPIDJoints; output++)
00306 if(state->pids[output][0]==0 && state->pids[output][1]==0 && state->pids[output][2]==0)
00307 cmdSums[output]=state->outputs[output];
00308
00309
00310 std::list<MC_ID> unlocked;
00311 for(MC_ID it=begin(); it!=end(); it=next(it))
00312 unlocked.push_back(it);
00313 unsigned int lastProcessed=get_time();
00314 while(unlocked.size()>0) {
00315 for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
00316 MotionCommand* mc=checkoutMotion(*it,false);
00317 if(mc==NULL)
00318 it++;
00319 else {
00320
00321 cur_cmd=*it;
00322 if(mc->shouldPrune()) {
00323 cout << "Removing expired " << *it << " (autoprune)" << endl;
00324 removeMotion(*it);
00325 } else
00326 mc->updateOutputs();
00327 cur_cmd=invalid_MC_ID;
00328 checkinMotion(*it);
00329
00330 std::list<MC_ID>::iterator rem=it++;
00331 unlocked.erase(rem);
00332 lastProcessed=get_time();
00333 }
00334 }
00335 if(get_time()-lastProcessed>FrameTime*NumFrames/2)
00336 break;
00337 }
00338 if(unlocked.size()>0) {
00339 cerr << "Warning: MotionManager was unable to obtain a lock on MCs: ";
00340 for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); it++)
00341 cerr << *it << ' ';
00342 cerr << endl;
00343 cerr << (unlocked.size()>1?"They":"It") << " may have been left locked by a Behavior while it was busy." << endl;
00344 cerr << "Try reducing the scope of your MMAccessor or call checkinMotion sooner." << endl;
00345 }
00346
00347
00348
00349 for(uint output=0; output<NumOutputs; output++) {
00350 cmdstatelist_t& curstatelist=cmdstates[output];
00351 for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
00352 MC_ID high_ent=bit;
00353 float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
00354 for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
00355 float curp=cmdlist[curstatelist[cit].mcid].priority;
00356 if(curp>high_p) {
00357 high_p=curp;
00358 high_ent=cit;
00359 }
00360 }
00361 curstatelist.swap(bit,high_ent);
00362
00363
00364
00365
00366 bit=high_ent;
00367 }
00368 }
00369
00370
00371
00372 for(uint frame=0; frame<NumFrames; frame++)
00373 for(uint output=0; output<NumOutputs; output++) {
00374 cmdstatelist_t& curstatelist=cmdstates[output];
00375 float alpha=1;
00376 OutputCmd sumcmd;
00377 cmdstatelist_t::index_t ent=curstatelist.begin();
00378 while(ent!=curstatelist.end() && alpha>0) {
00379 OutputCmd curcmd;
00380 float curp=curstatelist[ent].priority;
00381 float curalpha=1;
00382 for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00383
00384 float curweight=curstatelist[ent].frames[frame].weight;
00385 ASSERT(curweight>=0,"negative output weights are illegal");
00386 if(curweight<0) {
00387 cout << "weight=" << curweight << endl;
00388 curweight=0;
00389 }
00390 curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
00391 curcmd.weight+=curweight;
00392 if(curweight<1)
00393 curalpha*=(1-curweight);
00394 else
00395 curalpha=0;
00396 }
00397 if(curcmd.weight>0) {
00398
00399 sumcmd.value+=curcmd.value/curcmd.weight*(1-curalpha);
00400 sumcmd.weight+=(1-curalpha);
00401 alpha*=curalpha;
00402 }
00403 }
00404 if(sumcmd.weight>0)
00405 outputs[frame][output]=sumcmd.value/sumcmd.weight;
00406 else
00407 sumcmd.value=outputs[frame][output]=cmdSums[output];
00408 if(frame==NumFrames-1)
00409 cmds[output]=sumcmd;
00410 }
00411
00412 for(uint output=0; output<NumOutputs; output++)
00413 cmdSums[output]=outputs[NumFrames-1][output];
00414
00415
00416 for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
00417 cmdstatelist_t& curstatelist=cmdstates[output];
00418 float alpha=1;
00419 float sumpid[3];
00420 for(uint i=0; i<3; i++)
00421 sumpid[i]=0;
00422 float sumweight=0;
00423 cmdstatelist_t::index_t ent=curstatelist.begin();
00424 while(ent!=curstatelist.end() && alpha>0) {
00425 float tmppid[3];
00426 for(uint i=0; i<3; i++)
00427 tmppid[i]=0;
00428 float tmpweight=0;
00429 float curp=curstatelist[ent].priority;
00430 float curalpha=1;
00431 for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00432
00433 float curweight=curstatelist[ent].pid.weight;
00434 ASSERT(curweight>=0,"negative PID weights are illegal")
00435 if(curweight<0)
00436 curweight=0;
00437 for(uint i=0; i<3; i++)
00438 tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
00439 tmpweight+=curweight;
00440 if(curweight<1)
00441 curalpha*=(1-curweight);
00442 else
00443 curalpha=0;
00444 }
00445 if(tmpweight>0) {
00446
00447 for(uint i=0; i<3; i++)
00448 sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
00449 sumweight+=(1-curalpha);
00450 alpha*=curalpha;
00451 }
00452 }
00453 if(sumweight>0) {
00454 for(uint i=0; i<3; i++)
00455 sumpid[i]/=sumweight;
00456 setPID(output,sumpid);
00457 }
00458 }
00459
00460 func_end();
00461
00462
00463 }
00464
00465 void
00466 MotionManager::updateWorldState() {
00467
00468 for(uint output=LEDOffset; output<LEDOffset+NumLEDs; output++)
00469 state->outputs[output]=cmdSums[output];
00470 for(uint output=BinJointOffset; output<BinJointOffset+NumBinJoints; output++)
00471 state->outputs[output]=cmdSums[output];
00472
00473
00474
00475
00476 if(state->robotDesign & WorldState::ERS210Mask) {
00477 for(uint output=0; output<NumPIDJoints; output++)
00478 if(!ERS210Info::IsRealERS210[output])
00479 state->outputs[output]=cmdSums[output];
00480 } else if(state->robotDesign & WorldState::ERS220Mask) {
00481 for(uint output=0; output<NumPIDJoints; output++)
00482 if(!ERS220Info::IsRealERS220[output])
00483 state->outputs[output]=cmdSums[output];
00484 } else if(state->robotDesign & WorldState::ERS7Mask) {
00485 for(uint output=0; output<NumPIDJoints; output++)
00486 if(!ERS7Info::IsRealERS7[output])
00487 state->outputs[output]=cmdSums[output];
00488 } else
00489 cout << "MotionManager::updateWorldState() - could not detect model" << endl;
00490 }
00491
00492 #ifdef PLATFORM_APERIOS
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520 bool
00521 MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
00522
00523 bool dirty=!pidchanges.empty();
00524 while(!pidchanges.empty()) {
00525 float gain[3];
00526 word shift[3];
00527
00528
00529 for(uint i=0; i<3; i++) {
00530 shift[i]=DefaultPIDShifts[i];
00531 gain[i]=pidchanges.front().pids[i]*(1<<(0x10-shift[i]));
00532 }
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559 OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
00560 for(uint i=0; i<3; i++)
00561 state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
00562 pidchanges.pop_front();
00563 }
00564 return dirty;
00565 }
00566
00567 void
00568 MotionManager::receivedMsg(const ONotifyEvent& event) {
00569
00570 func_begin();
00571
00572
00573 for(int x=0; x<event.NumOfData(); x++)
00574 processMsg(event.RCData(x));
00575
00576 func_end();
00577 }
00578
00579 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00580
00581 void
00582 MotionManager::processMsg(RCRegion * rcr) {
00583
00584 if(rcr==NULL) {
00585 cout << "WARNING: MotionManager::processMsg was given a NULL region" << endl;
00586 return;
00587 }
00588 func_begin();
00589
00590 MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00591 MC_ID mc_id=mminfo->mc_id;
00592 switch(mminfo->type) {
00593 case MotionManagerMsg::addMotion: {
00594
00595 rcr->AddReference();
00596
00597 cmdlist[mc_id].rcr[_MMaccID]=rcr;
00598
00599
00600
00601 cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast<MotionCommand*>(mminfo);
00602 cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart();
00603 erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,0));
00604 } break;
00605 case MotionManagerMsg::deleteMotion: {
00606
00607
00608 if(cmdlist[mc_id].rcr[_MMaccID]==NULL)
00609 cout << "WARNING: MotionManager attempted to delete a NULL motion! mc_id="<<mc_id<<" _MMaccID=" << _MMaccID << endl;
00610 else {
00611 checkoutMotion(mc_id,true);
00612 cmdlist[mc_id].baseaddrs[_MMaccID]->DoStop();
00613 erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::deactivateETID,0));
00614 cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00615 cmdlist[mc_id].rcr[_MMaccID]=NULL;
00616 bool found=false;
00617 for(unsigned int i=0; i<numAcc; i++)
00618 if(cmdlist[mc_id].rcr[i]!=NULL) {
00619 found=true;
00620 break;
00621 }
00622 if(!found) {
00623 push_free(mc_id);
00624
00625
00626 } else {
00627 cout << "Warning: motion command " << mc_id << " still has attachments after delete message received" << endl;
00628 checkinMotion(mc_id);
00629 }
00630 }
00631
00632 } break;
00633 default:
00634 printf("*** WARNING *** unknown MotionManager msg type received\n");
00635 }
00636
00637 func_end();
00638 }
00639
00640
00641
00642
00643
00644
00645 MotionManager::MC_ID
00646 MotionManager::doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority) {
00647 MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
00648 if(mc==NULL) {
00649 cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
00650 return invalid_MC_ID;
00651 }
00652 mc->setAutoPrune(autoprune);
00653
00654 while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; }
00655 func_begin();
00656
00657
00658 for(MC_ID it=cmdlist.begin(); it!=cmdlist.end(); it=cmdlist.next(it)) {
00659 if(cmdlist[it].baseaddrs[_MMaccID]==mc) {
00660 cerr << "Warning: re-added motion command " << it << endl;
00661 return it;
00662 }
00663 }
00664 MC_ID mc_id = pop_free();
00665 if(mc_id==cmdlist.end()) {
00666 cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
00667 return func_end(cmdlist.end());
00668 }
00669 cmdlist[mc_id].baseaddrs[_MMaccID]=mc;
00670 cmdlist[mc_id].rcr[_MMaccID]=sm.getRegion();
00671
00672 cmdlist[mc_id].rcr[_MMaccID]->AddReference();
00673
00674 cmdlist[mc_id].lastAccessor=_MMaccID;
00675 cmdlist[mc_id].priority=priority;
00676
00677 mc->setAdd(mc_id);
00678 #ifdef PLATFORM_APERIOS
00679 OStatus err;
00680
00681
00682
00683
00684
00685
00686
00687
00688 err=subjs[_MMaccID]->SetData(sm.getRegion());
00689 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00690
00691 err=subjs[_MMaccID]->NotifyObservers();
00692 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00693 #else //PLATFORM_LOCAL
00694 subjs[_MMaccID]->sendMessage(sm.getRegion());
00695 #endif //PLATFORM check for IPC stuff
00696
00697
00698 return func_end(mc_id);
00699 }
00700
00701 MotionCommand *
00702 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00703
00704 if(mcid>=MAX_MOTIONS) {
00705 cout << "*** WARNING *** " << _MMaccID << " tried to access invalid mcid " << mcid << endl;
00706 return NULL;
00707 }
00708 if(block)
00709 cmdlist[mcid].lock.lock(_MMaccID);
00710 else
00711 if(!cmdlist[mcid].lock.try_lock(_MMaccID))
00712 return NULL;
00713 if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00714 cout << "*** WARNING *** " << _MMaccID << " tried to access dead mcid " << mcid << endl;
00715 cmdlist[mcid].lock.unlock();
00716 return NULL;
00717 }
00718
00719 MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
00720
00721 if(cmdlist[mcid].lastAccessor!=_MMaccID) {
00722
00723
00724
00725
00726 cmdlist[mcid].lastAccessor=_MMaccID;
00727 }
00728 base->setTranslator(etrans);
00729
00730 return base;
00731 }
00732
00733 void
00734 MotionManager::checkinMotion(MC_ID mcid) {
00735 cmdlist[mcid].baseaddrs[_MMaccID]->setTranslator(NULL);
00736 if(mcid!=invalid_MC_ID)
00737 cmdlist[mcid].lock.unlock();
00738 }
00739
00740 void
00741 MotionManager::removeMotion(MC_ID mcid) {
00742 if(mcid==invalid_MC_ID)
00743 return;
00744 func_begin();
00745 #ifdef PLATFORM_APERIOS
00746 MotionManagerMsg dmsg;
00747 dmsg.setDelete(mcid);
00748
00749 subjs[_MMaccID]->SetData(&dmsg,sizeof(dmsg));
00750 subjs[_MMaccID]->NotifyObservers();
00751 #else //PLATFORM_LOCAL
00752 SharedObject<MotionManagerMsg> dmsg;
00753 dmsg->setDelete(mcid);
00754
00755 subjs[_MMaccID]->sendMessage(dmsg.getRegion());
00756 #endif //PLATFORM check for IPC stuff
00757
00758
00759 if(cmdlist[mcid].rcr[_MMaccID]!=NULL) {
00760 cmdlist[mcid].rcr[_MMaccID]->RemoveReference();
00761 cmdlist[mcid].rcr[_MMaccID]=NULL;
00762 }
00763
00764 func_end();
00765 }
00766
00767
00768
00769
00770
00771
00772 void
00773 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00774 func_begin();
00775
00776
00777 for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00778 if(pidchanges[u].joint==joint) {
00779 for(uint i=0; i<3; i++) {
00780 pidchanges[u].pids[i]=pids[i];
00781 if(pids[i]!=state->pids[joint][i]) {
00782 for(i++; i<3; i++)
00783 pidchanges[u].pids[i]=pids[i];
00784 func_end();
00785 return;
00786 }
00787 }
00788
00789
00790 pidchanges.erase(u);
00791 func_end();
00792 return;
00793 }
00794 }
00795
00796
00797 for(uint i=0; i<3; i++)
00798 if(pids[i]!=state->pids[joint][i]) {
00799 PIDUpdate update(joint,pids);
00800 pidchanges.push_back(update);
00801
00802
00803
00804 break;
00805 }
00806 func_end();
00807 }
00808
00809
00810 MotionManager::MC_ID
00811 MotionManager::skip_ahead(MC_ID mcid) const {
00812
00813
00814 while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[_MMaccID]==NULL)
00815 mcid=cmdlist.next(mcid);
00816 return mcid;
00817 }
00818
00819 MotionManager::OutputState::OutputState()
00820 : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
00821 {}
00822 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
00823 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00824 {
00825 for(unsigned int i=0; i<NumFrames; i++)
00826 frames[i]=cmds[i];
00827 }
00828 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
00829 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00830 {
00831 for(unsigned int i=0; i<NumFrames; i++)
00832 frames[i]=cmd;
00833 }
00834 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
00835 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00836 {
00837 frames[frame]=cmd;
00838 }
00839 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputPID& p)
00840 : priority(pri),mcid(mc), pid(p)
00841 {}
00842 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
00843 : priority(pri),mcid(mc), pid(p)
00844 {
00845 for(unsigned int i=0; i<NumFrames; i++)
00846 frames[i]=cmds[i];
00847 }
00848 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
00849 : priority(pri),mcid(mc), pid(p)
00850 {
00851 for(unsigned int i=0; i<NumFrames; i++)
00852 frames[i]=cmd;
00853 }
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
|