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