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
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
00546 mc->setAdd(mc_id);
00547 OStatus err;
00548
00549
00550
00551
00552
00553
00554
00555
00556 err=subjs[_MMaccID]->SetData(sm.getRegion());
00557 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00558
00559 err=subjs[_MMaccID]->NotifyObservers();
00560 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00561
00562
00563 return func_end(mc_id);
00564 }
00565
00566 void
00567 MotionManager::receivedMsg(const ONotifyEvent& event) {
00568
00569 func_begin();
00570
00571
00572 for(int x=0; x<event.NumOfData(); x++) {
00573 RCRegion * rcr = event.RCData(x);
00574 MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00575 MC_ID mc_id=mminfo->mc_id;
00576 switch(mminfo->type) {
00577 case MotionManagerMsg::addMotion: {
00578
00579 rcr->AddReference();
00580
00581 cmdlist[mc_id].rcr[_MMaccID]=rcr;
00582
00583
00584
00585 cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast<MotionCommand*>(mminfo);
00586 cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart();
00587 erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,0));
00588 } break;
00589 case MotionManagerMsg::deleteMotion: {
00590
00591
00592 if(cmdlist[mc_id].rcr[_MMaccID]==NULL)
00593 cout << "WARNING: MotionManager attempted to delete a NULL motion! mc_id="<<mc_id<<" _MMaccID=" << _MMaccID << endl;
00594 else {
00595 checkoutMotion(mc_id,true);
00596 cmdlist[mc_id].baseaddrs[_MMaccID]->DoStop();
00597 erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::deactivateETID,0));
00598 cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00599 cmdlist[mc_id].rcr[_MMaccID]=NULL;
00600 push_free(mc_id);
00601 checkinMotion(mc_id);
00602 }
00603
00604 } break;
00605 default:
00606 printf("*** WARNING *** unknown MotionManager msg type received\n");
00607 }
00608 }
00609
00610 func_end();
00611 }
00612
00613 #endif //PLATFORM_APERIOS
00614
00615 MotionCommand *
00616 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00617
00618 if(mcid>=MAX_MOTIONS) {
00619 cout << "*** WARNING *** " << _MMaccID << " tried to access invalid mcid " << mcid << endl;
00620 return NULL;
00621 }
00622 if(block)
00623 cmdlist[mcid].lock.lock(_MMaccID);
00624 else
00625 if(!cmdlist[mcid].lock.try_lock(_MMaccID))
00626 return NULL;
00627 if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00628 cout << "*** WARNING *** " << _MMaccID << " tried to access dead mcid " << mcid << endl;
00629 cmdlist[mcid].lock.release();
00630 return NULL;
00631 }
00632
00633 MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
00634
00635 if(cmdlist[mcid].lastAccessor!=_MMaccID) {
00636
00637
00638
00639
00640 cmdlist[mcid].lastAccessor=_MMaccID;
00641 }
00642
00643 return base;
00644 }
00645
00646 void
00647 MotionManager::checkinMotion(MC_ID mcid) {
00648 if(mcid!=invalid_MC_ID)
00649 cmdlist[mcid].lock.release();
00650 }
00651
00652 void
00653 MotionManager::removeMotion(MC_ID mcid) {
00654 if(mcid==invalid_MC_ID)
00655 return;
00656 func_begin();
00657 #ifdef PLATFORM_APERIOS
00658 MotionManagerMsg dmsg;
00659 dmsg.setDelete(mcid);
00660
00661 subjs[_MMaccID]->SetData(&dmsg,sizeof(dmsg));
00662 subjs[_MMaccID]->NotifyObservers();
00663
00664
00665 cmdlist[mcid].rcr[_MMaccID]->RemoveReference();
00666
00667 #endif //PLATFORM_APERIOS
00668 func_end();
00669 }
00670
00671
00672
00673
00674
00675
00676 void
00677 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00678 func_begin();
00679
00680
00681 for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00682 if(pidchanges[u].joint==joint) {
00683 for(uint i=0; i<3; i++) {
00684 pidchanges[u].pids[i]=pids[i];
00685 if(pids[i]!=state->pids[joint][i]) {
00686 for(i++; i<3; i++)
00687 pidchanges[u].pids[i]=pids[i];
00688 func_end();
00689 return;
00690 }
00691 }
00692
00693
00694 pidchanges.erase(u);
00695 func_end();
00696 return;
00697 }
00698 }
00699
00700
00701 for(uint i=0; i<3; i++)
00702 if(pids[i]!=state->pids[joint][i]) {
00703 PIDUpdate update(joint,pids);
00704 pidchanges.push_back(update);
00705 break;
00706 }
00707 func_end();
00708 }
00709
00710
00711 MotionManager::MC_ID
00712 MotionManager::skip_ahead(MC_ID mcid) const {
00713
00714
00715 #ifdef PLATFORM_APERIOS
00716 while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[_MMaccID]==NULL)
00717 mcid=cmdlist.next(mcid);
00718 return mcid;
00719 #else
00720 return cmdlist.next(mcid);
00721 #endif
00722 }
00723
00724 MotionManager::OutputState::OutputState()
00725 : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
00726 {}
00727 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
00728 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00729 {
00730 for(unsigned int i=0; i<NumFrames; i++)
00731 frames[i]=cmds[i];
00732 }
00733 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
00734 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00735 {
00736 for(unsigned int i=0; i<NumFrames; i++)
00737 frames[i]=cmd;
00738 }
00739 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
00740 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00741 {
00742 frames[frame]=cmd;
00743 }
00744 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputPID& p)
00745 : priority(pri),mcid(mc), pid(p)
00746 {}
00747 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
00748 : priority(pri),mcid(mc), pid(p)
00749 {
00750 for(unsigned int i=0; i<NumFrames; i++)
00751 frames[i]=cmds[i];
00752 }
00753 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
00754 : priority(pri),mcid(mc), pid(p)
00755 {
00756 for(unsigned int i=0; i<NumFrames; i++)
00757 frames[i]=cmd;
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
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
|