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