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