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