Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

MotionManager.cc

Go 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 
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 //! just for convenience
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   //  cout << "ID is now " << _MMaccID << " of " << numAcc << endl;
00050   //  cout << "_MMaccID is " << &_MMaccID << endl;
00051   //  cout << "numAcc is " << &numAcc << " from " << this << endl;
00052   MMlock.lock(_MMaccID);
00053   //  accRegs[accID].init();
00054   subjs[_MMaccID]=subj;
00055   if(cmdlist.size()>0) //Shouldn't happen - busy wait in addMotion
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     //!@todo should be able to set background pid
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 /*! What's worse? A plethora of functions which are only called, and only useful at one place,
00196  *  or a big massive function which doesn't pollute the namespace?  This is the latter, for
00197  *  better or worse. */
00198 void
00199 MotionManager::getOutputs(float outputs[NumFrames][NumOutputs]) {
00200   //  if(begin(id)!=end())
00201   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles..." << flush;
00202   if(state==NULL) {
00203     // we haven't gotten the WorldState memory region from Main yet, just set LEDs to a wierd pattern and leave
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     //  if(begin(id)!=end())
00211     //if(state && state->buttons[LFrPawOffset]) cout << "getangles-nostate-done..." << flush;
00212     return;
00213   }
00214   func_begin();
00215   //  if(begin(id)!=end())
00216   //  cout << id << "..." << flush;
00217   //  cout << "CHECKOUT..." << flush;
00218   for(uint output=0; output<NumOutputs; output++)
00219     cmdstates[output].clear();
00220 
00221   //  std::cout << "UPDATE..." << std::flush;
00222   std::list<MC_ID> unlocked;
00223   for(MC_ID it=begin(); it!=end(); it=next(it)) // check out all the MotionCommands (only one at a time tho)
00224     unlocked.push_back(it);
00225   while(unlocked.size()>0) { // keep cycling through all the locks we didn't get
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++; //we didn't get a lock, skip it (we'll keep looping until we get it)
00230       else {
00231         // we got a lock
00232         cur_cmd=*it;
00233         if(mc->shouldPrune()) {
00234           cout << "Removing expired" << endl;
00235           removeMotion(*it);
00236         } else
00237           mc->updateOutputs(); // the MotionCommand should make calls to setOutput from within here
00238         checkinMotion(*it); // release lock, done with motion
00239         // remove id from list of unprocessed motioncommands
00240         std::list<MC_ID>::iterator rem=it++;
00241         unlocked.erase(rem);
00242       }
00243     }
00244     cur_cmd=invalid_MC_ID;
00245   }
00246 
00247   // sort the list of requested outputs based on priority
00248   // (insertion sort, data structure is linked list)
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       /*if(curstatelist.countf()!=curstatelist.countb() || curstatelist.countf()!=curstatelist.size()) {
00263         cout << "LOST ONE! " << bit << ' ' << high_ent << endl;
00264         cout << curstatelist.countf() << ' ' << curstatelist.countb() << ' ' << curstatelist.size() << endl;
00265         }*/
00266       bit=high_ent;
00267     }
00268   }
00269 
00270   // now we've got, for each output, a list of requested values sorted by priority
00271   // summarize each output
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; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00282         for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00283           //weighted average within priority level
00284           float curweight=curstatelist[ent].frames[frame].weight;
00285           ASSERT(curweight>=0,"negative output weights are illegal");
00286           if(curweight<0) { //negative weights are illegal
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           //weighted average of priority levels
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 //if zero weight, hold last value
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   // now summarize each output's PID values (for those which use PID control)
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; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00331       for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00332         //weighted average within priority level
00333         float curweight=curstatelist[ent].pid.weight;
00334         ASSERT(curweight>=0,"negative PID weights are illegal")
00335         if(curweight<0) //negative weights are illegal
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         //weighted average of priority levels
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   //  if(begin(id)!=end())
00362   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles-done." << flush;
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++)      //NOTE: Should probably be NumOutputs for portability, but this is faster
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++)      //NOTE: Should probably be NumOutputs for portability, but this is faster
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 // documentation for this function is at the end of the file
00408 MotionManager::MC_ID
00409 MotionManager::addMotion(const SharedObjectBase& sm) {
00410   //  cout << "addMotion..." << flush;
00411   while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; } //Wait for everyone to register
00412   func_begin();
00413   //  cout << id << "..." << flush;
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   //  cout << "addMotion-done" << endl;
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   //  cout << "receivedMsg..." << flush;
00471   func_begin();
00472   //  cout << id << "..." << flush;
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       //should be able to do a nice dynamic cast instead of a static one
00482       // but it gives NULL for some reason - i blame having to do the fork trick
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   //  cout << "receivedMsg-done" << endl;
00495   func_end();
00496 }
00497 
00498 #endif //PLATFORM_APERIOS
00499 
00500 MotionCommand *
00501 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00502   //cout << "checkout..." << flush;
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   //cout << "locked..." << endl;
00518   MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
00519   //  cout << "base=" << base << "..." << flush;
00520   if(cmdlist[mcid].lastAccessor!=_MMaccID) {
00521     //cout << "converting from " << MCRegistrar::getRaw(base) << "..." << flush;
00522     //cout << "prev=" << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << "..." << flush;
00523     //    accRegs[id].convert(base);
00524     //cout << "to=" << MCRegistrar::getRaw(base) << ", " << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << endl;
00525     cmdlist[mcid].lastAccessor=_MMaccID;
00526   }
00527   //cout << "checkout-done..." << flush;
00528   return base;
00529 }
00530 
00531 void
00532 MotionManager::checkinMotion(MC_ID mcid) {
00533   cmdlist[mcid].lock.release();
00534 }
00535 
00536 //! @test do i do this right, or does it leak memory?
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 /*! Note that we don't actually set the PIDs in the system here, we just queue them up.
00559  *  PID changes seem to be an expensive operation, so may only want to clear the queue
00560  *  at some reduced rate (although that's not actually currently implemented, so right
00561  *  now there's no real benefit until that's done) */
00562 void
00563 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00564   func_begin();
00565   //see if there's already an update for this joint
00566   for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00567     if(pidchanges[u].joint==joint) { //found it
00568       for(uint i=0; i<3; i++) {
00569         pidchanges[i].pids[i]=pids[i];
00570         if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID
00571           for(i++; i<3; i++) //we aren't, copy over the rest
00572             pidchanges[i].pids[i]=pids[i];
00573           func_end();
00574           return;
00575         }
00576       }
00577       //if it didn't return within the loop, no difference was found from current state
00578       //so just delete the update
00579       pidchanges.erase(u);
00580       func_end();
00581       return;
00582     }
00583   }
00584   //if we didn't return from the loop, we didn't find an update for the joint
00585   for(uint i=0; i<3; i++) //check to see if it's different from the current
00586     if(pids[i]!=state->pids[joint][i]) {
00587       PIDUpdate update(joint,pids); //it is different, insert a new update
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   // this is in case a new motion has been added, but the current
00598   // process hasn't received its own copy yet, so should skip over them
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 /*out*/, float pri, MC_ID mc, const OutputPID& p)
00629   : priority(pri),mcid(mc), pid(p)
00630 {}
00631 MotionManager::OutputState::OutputState(unsigned int /*out*/, 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 /*out*/, 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 /*! @file
00646  * @brief Implements MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
00647  * @author ejt (Creator)
00648  *
00649  * $Author: ejt $
00650  * $Name: tekkotsu-1_4_1 $
00651  * $Revision: 1.22 $
00652  * $State: Exp $
00653  * $Date: 2003/06/12 04:16:43 $
00654  */
00655 
00656 
00657 /*
00658     for(uint f=0;f<NumFrames;f++)
00659       for(uint i=0; i<NumOutputs; i++)
00660         outputs[f][i]=0;
00661     const uint cyctime=128;
00662     uint ot=get_time()+3*cyctime;
00663     for(uint f=0;f<NumFrames;f++) {
00664       uint t=ot+f*FrameTime;
00665       outputs[f][TopBrLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00666       t-=cyctime;
00667       outputs[f][TopLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00668       outputs[f][TopRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00669       t-=cyctime;
00670       outputs[f][MidLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00671       outputs[f][MidRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00672       t-=cyctime;
00673       outputs[f][BotLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00674       outputs[f][BotRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00675     }
00676 */
00677 
00678   /*  for(uint output=TlRedLEDOffset-1; output<LEDOffset+NumLEDs-1; output++) {
00679     cmdstatelist_t& curstatelist=cmdstates[output];
00680     cout << "Out " << output << ": ";
00681     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00682       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00683     cout << endl;
00684     }*/
00685 
00686 
00687   /*  cout << get_time() << ' ' << size() << endl;
00688   for(uint output=TlRedLEDOffset; output<LEDOffset+NumLEDs-1; output++) {
00689     cmdstatelist_t& curstatelist=cmdstates[output];
00690     cout << "Out " << output << ": ";
00691     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00692       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00693     cout << endl;
00694   }
00695   */

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2