Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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 #include "Shared/StackTrace.h"
00006 #include "Shared/ProjectInterface.h"
00007 
00008 #ifndef PLATFORM_APERIOS
00009 #  include "IPC/MessageQueue.h"
00010 #  include "IPC/MessageReceiver.h"
00011 #  include "Shared/ProjectInterface.h"
00012 #endif
00013 
00014 #include "Shared/ERS210Info.h"
00015 #include "Shared/ERS220Info.h"
00016 #include "Shared/ERS7Info.h"
00017 #include "Shared/Config.h"
00018 
00019 #include <list>
00020 
00021 MotionManager * motman=NULL;
00022 int MotionManager::_MMaccID[ProcessID::NumProcesses];
00023 EventTranslator* MotionManager::etrans=NULL;
00024 
00025 const float MotionManager::kIgnoredPriority    =-1;
00026 const float MotionManager::kBackgroundPriority = 0;
00027 const float MotionManager::kLowPriority        = 5;
00028 const float MotionManager::kStdPriority        = 10;
00029 const float MotionManager::kHighPriority       = 50;
00030 const float MotionManager::kEmergencyPriority  = 100;
00031 
00032 using namespace std;
00033 
00034 //! just for convenience
00035 typedef unsigned int uint;
00036 
00037 MotionManager::MotionManager()
00038 : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock(),numAcc(0)
00039 {
00040   for(uint x=0; x<NumOutputs; x++)
00041     cmdSums[x]=0;
00042 }
00043 
00044 #ifdef PLATFORM_APERIOS
00045 
00046 void
00047 MotionManager::InitAccess(OSubject* subj) {
00048   if(numAcc==MAX_ACCESS) {
00049     printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00050     return;
00051   }
00052   _MMaccID[ProcessID::getID()]=numAcc++;
00053   //  cout << "ID is now " << getAccID() << " of " << numAcc << endl;
00054   MMlock.lock(getAccID());
00055   //  accRegs[accID].init();
00056   subjs[getAccID()]=subj;
00057   if(cmdlist.size()>0) //Shouldn't happen - busy wait in addMotion
00058     cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00059   MMlock.unlock();
00060 }
00061 
00062 #else //now PLATFORM_LOCAL
00063 
00064 void
00065 MotionManager::InitAccess(MessageQueueBase& mcbufq, Resource& behaviorLock) {
00066   if(numAcc==MAX_ACCESS) {
00067     printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00068     return;
00069   }
00070   _MMaccID[ProcessID::getID()]=numAcc++;
00071   //  cout << "ID is now " << getAccID() << " of " << numAcc << endl;
00072   MMlock.lock(getAccID());
00073   subjs[getAccID()]=&mcbufq;
00074   mcrecvs[getAccID()]=new MessageReceiver(*subjs[getAccID()],receivedMsg);
00075   procLocks[getAccID()]=&behaviorLock;
00076   if(cmdlist.size()>0) //Shouldn't happen - busy wait in doAddMotion
00077     cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00078   MMlock.unlock();
00079 }
00080 
00081 #endif //PLATFORM-specific initialization
00082 
00083 void
00084 MotionManager::RemoveAccess() {
00085 #ifndef PLATFORM_APERIOS
00086   //kill the message receiver before we get the motion manager lock
00087   // that way if there's a message pending, it can be processed instead of deadlocking
00088   mcrecvs[getAccID()]->finish();
00089   delete mcrecvs[getAccID()];
00090   mcrecvs[getAccID()]=NULL;
00091 #endif
00092   
00093   func_begin();
00094   for(MC_ID mc_id=cmdlist.begin(); mc_id!=cmdlist.end(); mc_id=cmdlist.next(mc_id)) {
00095     if(cmdlist[mc_id].rcr[getAccID()]!=NULL) {
00096       MotionCommand* mc=checkoutMotion(mc_id,true);
00097       int found=0;
00098       for(unsigned int i=0; i<numAcc; i++) {
00099         if(cmdlist[mc_id].rcr[i]!=NULL) {
00100           found++;
00101         }
00102       }
00103       cout << "Warning: " << ProcessID::getIDStr() << " dropping motion command " << mc_id << ", was active at shutdown " << (mc->getAutoPrune()?"(was set for autoprune)":"(leaked?)") << endl;
00104 #ifdef PLATFORM_APERIOS
00105       int refs=1;
00106 #else
00107       int refs=cmdlist[mc_id].rcr[getAccID()]->NumberOfLocalReference();
00108 #endif
00109       if(refs>1)
00110         cout << "Warning: " << ProcessID::getIDStr() << " still had " <<  refs-1 << " references to motion command " << mc_id << " as it was being dropped (these are now invalid)" << endl;
00111       for(int i=0; i<refs; ++i)
00112         cmdlist[mc_id].rcr[getAccID()]->RemoveReference();
00113       cmdlist[mc_id].rcr[getAccID()]=NULL;
00114       if(found==1) {
00115         MC_ID p=cmdlist.prev(mc_id);
00116         push_free(mc_id); //don't check in -- lock destructor will release the lock as the entry is destructed
00117         mc_id=p;
00118       } else {
00119         checkinMotion(mc_id);
00120       }
00121     }
00122   }
00123   func_end();
00124 }
00125 
00126 
00127 MotionManager::~MotionManager() {
00128   if(!cmdlist.empty()) {
00129     func_begin();
00130     cout << "WARNING: MotionManager destruction with MotionCommands still attached." << endl;
00131     while(!cmdlist.empty()) {
00132       MC_ID mc_id=cmdlist.begin();
00133       for(unsigned int i=0; i<numAcc; i++)
00134         if(cmdlist[mc_id].rcr[i]!=NULL)
00135           cout << "MC " << mc_id << " was still referenced by InitAccess caller #" << getAccID() << endl;
00136       push_free(mc_id);
00137     }
00138     func_end();
00139   }
00140 }
00141 
00142 void
00143 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
00144   if(output >= NumOutputs)
00145     return;
00146   if(cmd.weight<=0)
00147     return;
00148 
00149   if(caller==NULL || caller->getID()!=cur_cmd)
00150     func_begin();
00151   if(cur_cmd==invalid_MC_ID) {
00152     cmdSums[output]=cmd.value;
00153   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00154     cmdstatelist_t& curstatelist=cmdstates[output];
00155     cmdstatelist_t::index_t ent=curstatelist.begin();
00156     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00157       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
00158     else
00159       for(unsigned int i=0; i<NumFrames; i++)
00160         curstatelist[ent].frames[i]=cmd;
00161   }
00162   if(caller==NULL || caller->getID()!=cur_cmd)
00163     func_end();
00164 }
00165 
00166 void
00167 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
00168   if(output >= NumOutputs)
00169     return;
00170   if(cmd.weight<=0)
00171     return;
00172 
00173   if(caller==NULL || caller->getID()!=cur_cmd)
00174     func_begin();
00175   if(cur_cmd==invalid_MC_ID) {
00176     cmdSums[output]=cmd.value;
00177   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00178     cmdstatelist_t& curstatelist=cmdstates[output];
00179     cmdstatelist_t::index_t ent=curstatelist.begin();
00180     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00181       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
00182     else
00183       curstatelist[ent].frames[frame]=cmd;
00184   }
00185   if(caller==NULL || caller->getID()!=cur_cmd)
00186     func_end();
00187 }
00188 
00189 void 
00190 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
00191   if(output >= NumOutputs)
00192     return;
00193   unsigned int hasWeight=NumFrames;
00194   for(unsigned int i=NumFrames-1; i!=-1U; i--)
00195     if(ocmds[i].weight>0) {
00196       hasWeight=i;
00197       break;
00198     }
00199   if(hasWeight==NumFrames)
00200     return;
00201   
00202 
00203   if(caller==NULL || caller->getID()!=cur_cmd)
00204     func_begin();
00205   if(cur_cmd==invalid_MC_ID) {
00206     cmdSums[output]=ocmds[hasWeight].value;
00207   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00208     cmdstatelist_t& curstatelist=cmdstates[output];
00209     cmdstatelist_t::index_t ent=curstatelist.begin();
00210     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00211       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
00212     else
00213       for(unsigned int i=0; i<NumFrames; i++)
00214         curstatelist[ent].frames[i]=ocmds[i];
00215   }
00216   if(caller==NULL || caller->getID()!=cur_cmd)
00217     func_end();
00218 }
00219 
00220 void
00221 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
00222   if(output >= NumOutputs)
00223     return;
00224 
00225   if(caller==NULL || caller->getID()!=cur_cmd)
00226     func_begin();
00227   if(cur_cmd==invalid_MC_ID) {
00228     setPID(output,pid.pid);
00229   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00230     cmdstatelist_t& curstatelist=cmdstates[output];
00231     cmdstatelist_t::index_t ent=curstatelist.begin();
00232     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00233       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
00234     else
00235       curstatelist[ent].pid=pid;
00236   }
00237   if(caller==NULL || caller->getID()!=cur_cmd)
00238     func_end();
00239 }
00240 
00241 void
00242 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
00243   if(output >= NumOutputs)
00244     return;
00245 
00246   if(caller==NULL || caller->getID()!=cur_cmd)
00247     func_begin();
00248   if(cur_cmd==invalid_MC_ID) {
00249     if(cmd.weight>0)
00250       cmdSums[output]=cmd.value;
00251     setPID(output,pid.pid);
00252   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00253     cmdstatelist_t& curstatelist=cmdstates[output];
00254     cmdstatelist_t::index_t ent=curstatelist.begin();
00255     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00256       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
00257     else {
00258       for(unsigned int i=0; i<NumFrames; i++)
00259         curstatelist[ent].frames[i]=cmd;
00260       curstatelist[ent].pid=pid;
00261     }
00262   }
00263   if(caller==NULL || caller->getID()!=cur_cmd)
00264     func_end();
00265 }
00266 
00267 void
00268 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
00269   if(output >= NumOutputs)
00270     return;
00271 
00272   if(caller==NULL || caller->getID()!=cur_cmd)
00273     func_begin();
00274   if(cur_cmd==invalid_MC_ID) {
00275     if(ocmds[NumFrames-1].weight>0)
00276       cmdSums[output]=ocmds[NumFrames-1].value;
00277     setPID(output,pid.pid);
00278   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00279     cmdstatelist_t& curstatelist=cmdstates[output];
00280     cmdstatelist_t::index_t ent=curstatelist.begin();
00281     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00282       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds,pid));
00283     else {
00284       for(unsigned int i=0; i<NumFrames; i++)
00285         curstatelist[ent].frames[i]=ocmds[i];
00286       curstatelist[ent].pid=pid;
00287     }
00288   }
00289   if(caller==NULL || caller->getID()!=cur_cmd)
00290     func_end();
00291 }
00292 
00293 /*! What's worse? A plethora of functions which are only called, and only useful at one place,
00294  *  or a big massive function which doesn't pollute the namespace?  This is the latter, for
00295  *  better or worse. */
00296 void
00297 MotionManager::getOutputs(float outputs[][NumOutputs]) {
00298   //  if(begin(id)!=end())
00299   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles..." << flush;
00300   if(state==NULL) {
00301     // we haven't gotten the WorldState memory region from Main yet, just set LEDs to a wierd pattern and leave
00302     for(uint f=0;f<NumFrames;f++)
00303       for(uint i=0; i<NumOutputs; i++)
00304         outputs[f][i]=0;
00305     for(uint f=0;f<NumFrames;f++)
00306       for(uint l=0; l<NumLEDs; l++)
00307         outputs[f][l]=l/(NumLEDs-1.0);
00308     //  if(begin(id)!=end())
00309     //if(state && state->buttons[LFrPawOffset]) cout << "getangles-nostate-done..." << flush;
00310     return;
00311   }
00312   func_begin();
00313   //  if(begin(id)!=end())
00314   //  cout << id << "..." << flush;
00315   //cout << "CHECKOUT..." << flush;
00316   for(uint output=0; output<NumOutputs; output++)
00317     cmdstates[output].clear();
00318 
00319   // for each PID joint which is set to 0 power, set the background
00320   // position value to current sensed value this prevents jerking back
00321   // to the previous position when joint(s) are moved during 0 power,
00322   // and then power is turned back on. (power here is in the 0 pid
00323   // sense, the joints are still receiving power from the system -
00324   // that's a separate system call)
00325   // Note that we wouldn't want to do this all the time, because
00326   // miscalibration between the sensed position and target position
00327   // will cause joints to drift to the extremities of motion, in some
00328   // cases, very quickly, and in worse cases, colliding with other
00329   // joints
00330   for(uint output=0; output<NumPIDJoints; output++)
00331     if(state->pids[output][0]==0 && state->pids[output][1]==0 && state->pids[output][2]==0)
00332       cmdSums[output]=state->outputs[output];
00333 
00334   //std::cout << "UPDATE..." << std::flush;
00335   std::list<MC_ID> unlocked;
00336   for(MC_ID it=begin(); it!=end(); it=next(it)) // check out all the MotionCommands (only one at a time tho)
00337     if(cmdlist[it].lastAccessor!=(accID_t)-1)
00338       unlocked.push_back(it);
00339   unsigned int lastProcessed=get_time();
00340   while(unlocked.size()>0) { // keep cycling through all the locks we didn't get
00341     for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
00342       MotionCommand* mc=checkoutMotion(*it,false);
00343       if(mc==NULL)
00344         it++; //we didn't get a lock, skip it (we'll keep looping until we get it)
00345       else {
00346         // we got a lock
00347         cur_cmd=*it;
00348         bool prune=true;
00349         try {
00350           prune=mc->shouldPrune();
00351         } catch(const std::exception& ex) {
00352           ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand prune test, will prune",&ex);
00353         } catch(...) {
00354           ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand prune test, will prune",NULL);
00355         }
00356         if(prune) {
00357           cout << "Removing expired " << *it << " (autoprune)" << endl;
00358           checkinMotion(*it); // release lock, done with motion (don't need to (and shouldn't) keep lock through the removeMotion())
00359           //only the last process to receive the remove notification actually does the remove, and
00360           //wouldn't be able to undo the thread portion of the lock made in this process
00361           //so we have to take off our own lock here first.
00362           removeMotion(*it);
00363         } else {
00364           try {
00365             mc->updateOutputs(); // the MotionCommand should make calls to setOutput from within here
00366           } catch(const std::exception& ex) {
00367             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand updateOutputs",&ex);
00368           } catch(...) {
00369             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand updateOutputs",NULL);
00370           }
00371           checkinMotion(*it); // release lock, done with motion
00372         }
00373         cur_cmd=invalid_MC_ID;
00374         // remove id from list of unprocessed motioncommands
00375         std::list<MC_ID>::iterator rem=it++;
00376         unlocked.erase(rem);
00377         lastProcessed=get_time();
00378       }
00379     }
00380     if(get_time()-lastProcessed>FrameTime*NumFrames/2)
00381       break;
00382   }
00383   if(unlocked.size()>0) {
00384     cerr << "Warning: MotionManager was unable to obtain a lock on MCs: ";
00385     for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); it++)
00386       cerr << *it << ' ';
00387     cerr << endl;
00388     cerr << (unlocked.size()>1?"They":"It") << " may have been left locked by a Behavior while it was busy." << endl;
00389     cerr << "Try reducing the scope of your MMAccessor or call checkinMotion sooner." << endl;
00390   }
00391 
00392   // sort the list of requested outputs based on priority
00393   // (insertion sort, data structure is linked list)
00394   for(uint output=0; output<NumOutputs; output++) {
00395     cmdstatelist_t& curstatelist=cmdstates[output];
00396     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
00397       MC_ID high_ent=bit;
00398       float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
00399       for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
00400         float curp=cmdlist[curstatelist[cit].mcid].priority;
00401         if(curp>high_p) {
00402           high_p=curp;
00403           high_ent=cit;
00404         }
00405       }
00406       curstatelist.swap(bit,high_ent);
00407       /*if(curstatelist.countf()!=curstatelist.countb() || curstatelist.countf()!=curstatelist.size()) {
00408         cout << "LOST ONE! " << bit << ' ' << high_ent << endl;
00409         cout << curstatelist.countf() << ' ' << curstatelist.countb() << ' ' << curstatelist.size() << endl;
00410         }*/
00411       bit=high_ent;
00412     }
00413   }
00414 
00415   // now we've got, for each output, a list of requested values sorted by priority
00416   // summarize each output
00417   for(uint frame=0; frame<NumFrames; frame++) {
00418     for(uint output=0; output<NumOutputs; output++) {
00419       cmdstatelist_t& curstatelist=cmdstates[output];
00420       float alpha=1;
00421       OutputCmd sumcmd;
00422       /*if(curstatelist.size()>1) {
00423         cout << get_time() << "." << frame << " " << outputNames[output] << ": ";
00424         for(cmdstatelist_t::index_t ent=curstatelist.begin(); ent!=curstatelist.end(); ent=curstatelist.next(ent))
00425           cout << "  (" << curstatelist[ent].frames[frame].value <<',' << curstatelist[ent].frames[frame].weight << ',' << curstatelist[ent].priority << ')';
00426         cout << endl;
00427       }*/
00428       cmdstatelist_t::index_t ent=curstatelist.begin();
00429       while(ent!=curstatelist.end() && alpha>0) {
00430         OutputCmd curcmd;
00431         float curp=curstatelist[ent].priority;
00432         float curalpha=1; // curalpha is cumulative product of leftovers (weights between 0 and 1)
00433         for(;ent!=curstatelist.end() && curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00434           //weighted average within priority level
00435           float curweight=curstatelist[ent].frames[frame].weight;
00436           ASSERT(curweight>=0,"negative output weights are illegal, MC_ID="<<curstatelist[ent].mcid<<" joint="<<outputNames[output]<<" frame="<<frame<<" weight="<<curweight);
00437           if(curweight<0) //negative weights are illegal
00438             curweight=0;
00439           curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
00440           curcmd.weight+=curweight;
00441           if(curweight<1)
00442             curalpha*=(1-curweight);
00443           else
00444             curalpha=0;
00445         }
00446         if(curcmd.weight>0) {
00447           //weighted average of priority levels
00448           sumcmd.value+=curcmd.value/curcmd.weight*alpha*(1-curalpha);
00449           sumcmd.weight+=alpha*(1-curalpha);
00450           alpha*=curalpha;
00451         }
00452       }
00453 
00454       //if(curstatelist.size()>1)
00455       //  cout << "   -> " << sumcmd.value/sumcmd.weight << ',' << sumcmd.weight << " @ " << alpha << endl;
00456       if(sumcmd.weight>0) {
00457         sumcmd.value/=sumcmd.weight;
00458         outputs[frame][output]=sumcmd.value;
00459       } else //if zero weight, hold last value
00460         sumcmd.value=outputs[frame][output]=cmdSums[output];
00461       if(frame==NumFrames-1)
00462         cmds[output]=sumcmd;
00463     }
00464   }
00465   
00466   for(uint output=0; output<NumOutputs; output++)
00467     cmdSums[output]=outputs[NumFrames-1][output];
00468 
00469   for (uint frame_idx = 0; frame_idx < NumFrames; frame_idx++) {
00470     for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++)
00471       outputs[frame_idx][output] = (outputs[frame_idx][output] + config->motion.calibration_offset[output-PIDJointOffset])
00472         * config->motion.calibration_scale[output-PIDJointOffset];
00473   }
00474       
00475         
00476   // now summarize each output's PID values (for those which use PID control)
00477   for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
00478     cmdstatelist_t& curstatelist=cmdstates[output];
00479     float alpha=1;
00480     float sumpid[3];
00481     for(uint i=0; i<3; i++)
00482       sumpid[i]=0;
00483     float sumweight=0;
00484     cmdstatelist_t::index_t ent=curstatelist.begin();
00485     while(ent!=curstatelist.end() && alpha>0) {
00486       float tmppid[3];
00487       for(uint i=0; i<3; i++)
00488         tmppid[i]=0;
00489       float tmpweight=0;
00490       float curp=curstatelist[ent].priority;
00491       float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00492       for(;ent!=curstatelist.end() && curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00493         //weighted average within priority level
00494         float curweight=curstatelist[ent].pid.weight;
00495         ASSERT(curweight>=0,"negative PID weights are illegal")
00496         if(curweight<0) //negative weights are illegal
00497           curweight=0;
00498         for(uint i=0; i<3; i++)
00499           tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
00500         tmpweight+=curweight;
00501         if(curweight<1)
00502           curalpha*=(1-curweight);
00503         else
00504           curalpha=0;
00505       }
00506       if(tmpweight>0) {
00507         //weighted average of priority levels
00508         for(uint i=0; i<3; i++)
00509           sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
00510         sumweight+=(1-curalpha);
00511         alpha*=curalpha;
00512       }
00513     }
00514     if(sumweight>0) {
00515       for(uint i=0; i<3; i++)
00516         sumpid[i]/=sumweight;
00517       setPID(output,sumpid);
00518     }
00519   }
00520 
00521   func_end();
00522   //  if(begin(id)!=end())
00523   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles-done." << flush;
00524 }
00525 
00526 /*! This function handles the conversion from the Tekkotsu format (one
00527  *  regular IEEE float per parameter, to the OPEN-R format (which
00528  *  takes a specialized, reduced precision floating point number) This
00529  *  is all documented in PIDMC as well.
00530  *
00531  *  In order to send Tekkotsu's PIDs to the system, they are converted
00532  *  to the gain/shift format.  On the ERS-2xx, we could dynamically
00533  *  choose shift values to allow more precision in setting values.
00534  *
00535  *  With the ERS-7, all shifts are shared, so they must be set to a
00536  *  common set of values, defined by WorldState::DefaultPIDShifts.
00537  *  This limits the range of gains which can then be set.
00538  *
00539  *  Due to the mysterious warning which would occur with the 2xx,
00540  *  (AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.)  and
00541  *  since this seems to be the way things are going, all models now,
00542  *  by default, use global shift values (which can vary from model to
00543  *  model, just global for each model)
00544  *
00545  *  You can revert to the dynamic shift selection by commenting-in
00546  *  the noted code section below.
00547  *
00548  *  A final note: the OPENR::SetJointGain function seems to be
00549  *  a rather costly function call.  You should probably try to avoid
00550  *  setting PIDs at too high a frequency.
00551  */
00552 bool
00553 #ifdef PLATFORM_APERIOS
00554 MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
00555 #else
00556 MotionManager::updatePIDs() {
00557 #endif
00558   //cout << "updatePIDs " << endl;
00559   bool dirty=!pidchanges.empty();
00560   while(!pidchanges.empty()) {
00561 #ifdef PLATFORM_APERIOS
00562     float gain[3];
00563     word shift[3];
00564 
00565     //if you want to enforce the default shifts:
00566     for(uint i=0; i<3; i++) {
00567       shift[i]=DefaultPIDShifts[i];
00568       gain[i]=pidchanges.front().pids[i]*(1<<(0x10-shift[i]));
00569     }
00570 
00571     //if you want to allow shifts to move for better precision:
00572     // this is OK on 2xx, although it occasionally produces warnings like:
00573     // AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.
00574     // It still seems to work fine though.
00575     // HOWEVER, the ERS-7 is a different story.  Apparently ( https://openr.aibo.com/cgi-bin/openr/e_regi/im_trbbs.cgi?uid=general&df=bbs.dat&prm=TAN&pg=1&no=0893#0893 )
00576     // all joints share the same shift, and so there must be one
00577     // global setting enforced.  If this is the way things are heading,
00578     // might as well just have everyone use the first method instead.
00579     // but here's the more dynamic way just for posterity:
00580     /*
00581     for(uint i=0; i<3; i++) {
00582       gain[i]=pidchanges.front().pids[i]*2;
00583       shift[i]=0xF;
00584       while(shift[i]!=2 && (gain[i]!=(word)gain[i] || gain[i]<=1) && gain[i]<0x20) {
00585         gain[i]*=2;
00586         shift[i]--;
00587       }
00588     }
00589     */
00590 
00591     //some debugging output (pick your favorite joint - i was having trouble with the ERS-7 nod joint in particular)
00592     //if(pidchanges.front().joint==HeadOffset+NodOffset)
00593     //cout << (word)gain[0] << ' ' << shift[0] << "   " << (word)gain[1] << ' ' << shift[1] << "   " << (word)gain[2] << ' ' << shift[2] << endl;
00594     //cout << gain[0] << ' ' << shift[0] << "   " << gain[1] << ' ' << shift[1] << "   " << gain[2] << ' ' << shift[2] << endl;
00595 
00596     OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
00597 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00598     for(uint i=0; i<3; i++)
00599       state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
00600     pidchanges.pop_front();
00601   }
00602   return dirty;
00603 }
00604 
00605 #ifdef PLATFORM_APERIOS
00606 void
00607 MotionManager::receivedMsg(const ONotifyEvent& event) {
00608   //  cout << "receivedMsg..." << flush;
00609   func_begin();
00610   //  cout << id << "..." << flush;
00611   //cout << "Received at " << get_time() << endl;
00612   for(int x=0; x<event.NumOfData(); x++)
00613     processMsg(event.RCData(x));
00614   //  cout << "receivedMsg-done" << endl;
00615   func_end();
00616 }
00617 #else //PLATFORM_LOCAL
00618 bool
00619 MotionManager::receivedMsg(RCRegion* msg) {
00620   try {
00621     MarkScope l(*motman->procLocks[getAccID()]);
00622     /*MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(msg->Base());
00623     if(mminfo->creatorPID==ProcessID::getID())
00624       return true; //don't process echos*/
00625     motman->processMsg(msg);
00626   } catch(const std::exception& ex) {
00627     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManagerMsg processing (MotionManager::receivedMsg)",&ex))
00628       throw;
00629   } catch(...) {
00630     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManagerMsg processing (MotionManager::receivedMsg)",NULL))
00631       throw;
00632   }
00633   return true;
00634 }
00635 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00636 
00637 void
00638 MotionManager::processMsg(RCRegion * rcr) {
00639   //cout << "processMsg..." << flush;
00640   if(rcr==NULL) {
00641     cout << "WARNING: MotionManager::processMsg was given a NULL region" << endl;
00642     return;
00643   }
00644   func_begin();
00645   //cout << ProcessID::getID() << "..." << flush;
00646   MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00647   MC_ID mc_id=mminfo->mc_id;
00648   if(mc_id==invalid_MC_ID) {
00649     //cout << "add message for already-deleted motion" << endl;
00650     func_end();
00651     return;
00652   }
00653   switch(mminfo->type) {
00654     case MotionManagerMsg::addMotion: {
00655       if(cmdlist[mc_id].rcr[getAccID()]==NULL) {
00656         //cout << "receiveMotion()NOW: rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00657         cmdlist[mc_id].rcr[getAccID()]=rcr;
00658         //cout << "receiveMotion(): rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00659         rcr->AddReference();
00660         //should be able to do a nice dynamic cast instead of a static one
00661         // but it gives NULL for some reason - i blame having to do the fork trick
00662         //cout << "Adding mc_id=="<< mc_id << " (and dynamic_cast is still " << dynamic_cast<MotionCommand*>(mminfo) << ")" << endl;
00663         cmdlist[mc_id].baseaddrs[getAccID()]=static_cast<MotionCommand*>(mminfo);
00664       } else {
00665 #ifdef PLATFORM_APERIOS
00666         // this shouldn't happen on Aperios (no echos), but is normal elsewhere
00667         cerr << "WARNING: MotionManager::processMsg addMotion for motion which was already added!" << endl;
00668 #else
00669         ASSERT(cmdlist[mc_id].lastAccessor==(accID_t)-1 || cmdlist[mc_id].baseaddrs[getAccID()]==static_cast<MotionCommand*>(mminfo),"Baseaddr changed!");
00670 #endif
00671       }
00672       if(ProcessID::getID()==ProcessID::MotionProcess && cmdlist[mc_id].lastAccessor!=(accID_t)-1) {
00673         checkoutMotion(mc_id,true);
00674         try {
00675           cmdlist[mc_id].baseaddrs[getAccID()]->DoStart();
00676         } catch(const std::exception& ex) {
00677           ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStart()",&ex);
00678         } catch(...) {
00679           ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStart()",NULL);
00680         }
00681         checkinMotion(mc_id);
00682       }
00683     } break;
00684     case MotionManagerMsg::deleteMotion: {
00685       //cout << "deleteMotion(): cmdlist[mc_id].rcr[getAccID()]->NumberOfReference()==" << cmdlist[mc_id].rcr[getAccID()]->NumberOfReference() << endl;
00686       //cout << "deleting mc_id=="<<mc_id << "..." << flush;
00687       ASSERT(cmdlist[mc_id].lastAccessor==(accID_t)-1,"delete motion message for motion command not marked for deletion");
00688       if(cmdlist[mc_id].rcr[getAccID()]==NULL) {
00689 #ifndef PLATFORM_APERIOS
00690         // on non-aperios, we get an echo of the remove request, so don't report an error, just ignore it
00691         if(mminfo->creatorPID!=ProcessID::getID()) {
00692 #endif
00693           cout << "WARNING: MotionManager attempted to delete a NULL motion! mc_id="<<mc_id<<" process=" << ProcessID::getIDStr() << endl;
00694           stacktrace::displayCurrentStackTrace();
00695 #ifndef PLATFORM_APERIOS
00696         }
00697 #endif
00698       } else {
00699         cmdlist[mc_id].lock.lock(getAccID());
00700         int found=0;
00701         for(unsigned int i=0; i<numAcc; i++) {
00702           if(cmdlist[mc_id].rcr[i]!=NULL) {
00703             found++;
00704           }
00705         }
00706         if(ProcessID::getID()==ProcessID::MotionProcess) {
00707           // no need to check out, motion should already be marked inaccessible from original call to removeMotion (as asserted at beginning of deleteMotion case)
00708           try {
00709             convertMotion(mc_id)->DoStop();
00710           } catch(const std::exception& ex) {
00711             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",&ex);
00712           } catch(...) {
00713             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",NULL);
00714           }
00715           cmdlist[mc_id].lastAccessor=(accID_t)-1; // reset inaccessiblity
00716         }
00717         cmdlist[mc_id].rcr[getAccID()]->RemoveReference();
00718         cmdlist[mc_id].rcr[getAccID()]=NULL;
00719         cmdlist[mc_id].baseaddrs[getAccID()]=NULL;
00720         if(found==1) {
00721           push_free(mc_id); //don't unlock -- lock destructor will release the lock as the entry is destructed
00722         } else {
00723           cmdlist[mc_id].lock.unlock();
00724         }
00725         if(ProcessID::getID()==ProcessID::MainProcess) {
00726           try {
00727             erouter->postEvent(EventBase::motmanEGID,mc_id,EventBase::deactivateETID,0);
00728           } catch(const std::exception& ex) {
00729             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00730           } catch(...) {
00731             ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00732           }
00733         }
00734       }
00735       //cout << "deleteMotion()NOW: cmdlist[mc_id].rcr[getAccID()]->NumberOfReference()==" << cmdlist[mc_id].rcr[getAccID()]->NumberOfReference() << endl;
00736     } break;
00737     default:
00738       printf("*** WARNING *** unknown MotionManager msg type received\n");
00739   }
00740   //cout << "processMsg-done" << endl;
00741   func_end();
00742 }
00743 
00744 
00745 /*! We have made this function protected because it's more readable if you
00746 *  call addPrunableMotion() or addPersistentMotion() instead... we decided
00747 *  requiring people to pass a true/false arguement wouldn't make it clear
00748 *  what that true/false was controlling. */
00749 MotionManager::MC_ID 
00750 MotionManager::doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority) {
00751   MotionCommand * mc = dynamic_cast<MotionCommand*>(static_cast<MotionManagerMsg*>(sm.data()));
00752   if(mc==NULL) {
00753     cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
00754     return invalid_MC_ID;
00755   }
00756   mc->setAutoPrune(autoprune);
00757   //cout << "addMotion...";
00758   while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; } //Wait for everyone to register
00759   func_begin();
00760   //cout << cmdlist.size() << " exist..." << endl;
00761   //  cout << id << "..." << flush;
00762   for(MC_ID it=cmdlist.begin(); it!=cmdlist.end(); it=cmdlist.next(it)) {
00763     if(cmdlist[it].baseaddrs[getAccID()]==mc) {
00764       cerr << "Warning: adding motion command at " << mc << ", is already running in motion manager as MC_ID " << it << endl;
00765       return func_end(it);
00766     }
00767   }
00768   MC_ID mc_id = pop_free();
00769   //cout << sm.getRegion()->ID().key << " holds " << mc_id << endl;
00770   if(mc_id==cmdlist.end()) {
00771     cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
00772     return func_end(cmdlist.end());
00773   }
00774   //cout << "setAdd(" << mc_id << ")" << endl;
00775   mc->setAdd(mc_id);
00776   cmdlist[mc_id].baseaddrs[getAccID()]=mc;
00777   cmdlist[mc_id].rcr[getAccID()]=sm.getRegion();
00778   //cout << "addMotion(): sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00779   cmdlist[mc_id].rcr[getAccID()]->AddReference();
00780   //cout << "addMotion()NOW: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00781   cmdlist[mc_id].lastAccessor=getAccID();
00782   cmdlist[mc_id].priority=priority;
00783   try {
00784     erouter->postEvent(EventBase::motmanEGID,mc_id,EventBase::activateETID,0);
00785   } catch(const std::exception& ex) {
00786     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00787   } catch(...) {
00788     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00789   }
00790   
00791 #ifdef PLATFORM_APERIOS
00792   OStatus err;
00793   /*{
00794     unsigned int i=0;
00795     for(ObserverConstIterator it=subjs[getAccID()]->begin();it!=subjs[getAccID()]->end();it++) {
00796       cout << "RemainBuffer("<<i++<<")==" << subjs[getAccID()]->RemainBuffer(*it) << endl;
00797     }
00798     ASSERT((int)i==subjs[getAccID()]->NumberOfObservers(),"did I miss an observer?");
00799   }*/
00800   //cout << "Sent at " << get_time() << flush;
00801   err=subjs[getAccID()]->SetData(sm.getRegion());
00802   ASSERT(err==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00803   //cout << "addMotion()afterSetData: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00804   err=subjs[getAccID()]->NotifyObservers();
00805   ASSERT(err==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00806 #else //PLATFORM_LOCAL
00807   try {
00808     subjs[getAccID()]->sendMessage(sm.getRegion());
00809   } catch(const std::exception& ex) {
00810     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",&ex);
00811   } catch(...) {
00812     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",NULL);
00813   }
00814 #endif //PLATFORM check for IPC stuff
00815   //  cout << "addMotion-done" << endl;
00816   //cout << " - " << get_time() << endl;
00817   return func_end(mc_id);
00818 }
00819 
00820 MotionCommand *
00821 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00822   //cout << "checkout..." << flush;
00823   if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID) {
00824     cout << "*** ERROR *** " << ProcessID::getIDStr() << " tried to access invalid mcid " << mcid << endl;
00825     stacktrace::displayCurrentStackTrace();
00826     return NULL;
00827   }
00828   if(cmdlist[mcid].lastAccessor==(accID_t)-1) { // test both before and after the block
00829     cout << "*** WARNING *** " << ProcessID::getIDStr() << " tried to access dead mcid " << mcid << endl;
00830     stacktrace::displayCurrentStackTrace();
00831     return NULL;
00832   }
00833   // block would lock up if the motion is dead in the simulator (left in a locked state)
00834   if(block)
00835     cmdlist[mcid].lock.lock(getAccID());
00836   else
00837     if(!cmdlist[mcid].lock.try_lock(getAccID()))
00838       return NULL;
00839   if(cmdlist[mcid].lastAccessor==(accID_t)-1) { // test both before and after the block
00840     cout << "*** WARNING *** " << ProcessID::getIDStr() << " tried to access mcid as it was removed " << mcid << endl;
00841     stacktrace::displayCurrentStackTrace();
00842     cmdlist[mcid].lock.unlock();
00843     return NULL;
00844   }
00845   //cout << "locked..." << endl;
00846   //cout << "checkout-done..." << flush;
00847   return convertMotion(mcid);
00848 }
00849 
00850 MotionCommand *
00851 MotionManager::convertMotion(MC_ID mc) {
00852   MotionCommand * base = cmdlist[mc].baseaddrs[getAccID()];
00853   //  cout << "base=" << base << "..." << flush;
00854   if(cmdlist[mc].lastAccessor!=getAccID()) {
00855     //cout << "converting from " << MCRegistrar::getRaw(base) << "..." << flush;
00856     //cout << "prev=" << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << "..." << flush;
00857     //    accRegs[id].convert(base);
00858     //cout << "to=" << MCRegistrar::getRaw(base) << ", " << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << endl;
00859     cmdlist[mc].lastAccessor=getAccID();
00860   }
00861   base->setTranslator(etrans);
00862 #ifdef PLATFORM_APERIOS
00863   base->setWorldState(state);
00864 #endif
00865   return base;
00866 }
00867 
00868 void
00869 MotionManager::checkinMotion(MC_ID mcid) {
00870   if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID)
00871     return;
00872   if(cmdlist[mcid].lock.get_lock_level()==1 && cmdlist[mcid].rcr[getAccID()]!=NULL) { //about to release last lock (and region hasn't been removed)
00873     MotionCommand * base = cmdlist[mcid].baseaddrs[getAccID()];
00874     base->setTranslator(NULL);
00875 #ifdef PLATFORM_APERIOS
00876     base->setWorldState(NULL);
00877 #endif
00878   }
00879   cmdlist[mcid].lock.unlock();
00880 }
00881 
00882 void
00883 MotionManager::removeMotion(MC_ID mcid) {
00884   if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID)
00885     return;
00886   func_begin();
00887   if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00888     cout << "WARNING: removeMotion called for a motion which has already been removed mc_id="<<mcid<<" process=" << ProcessID::getIDStr() << endl;
00889     stacktrace::displayCurrentStackTrace();
00890     func_end();
00891     return;
00892   }
00893   if(cmdlist[mcid].rcr[getAccID()]==NULL) { 
00894     cout << "WARNING: removeMotion called for a NULL motion! mc_id="<<mcid<<" process=" << ProcessID::getIDStr() << endl;
00895     stacktrace::displayCurrentStackTrace();
00896     func_end();
00897     return;
00898   }
00899   cmdlist[mcid].lock.lock(getAccID());
00900   if(ProcessID::getID()==ProcessID::MotionProcess) {
00901     MotionCommand * mc=checkoutMotion(mcid,true);
00902     try {
00903       mc->DoStop();
00904     } catch(const std::exception& ex) {
00905       ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",&ex);
00906     } catch(...) {
00907       ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",NULL);
00908     }
00909     checkinMotion(mcid);
00910   }
00911   cmdlist[mcid].lastAccessor=(accID_t)-1;
00912   cmdlist[mcid].rcr[getAccID()]->RemoveReference();
00913   cmdlist[mcid].rcr[getAccID()]=NULL;
00914   cmdlist[mcid].baseaddrs[getAccID()]=NULL;
00915   cmdlist[mcid].lock.unlock();
00916   if(ProcessID::getID()==ProcessID::MainProcess) {
00917     try {
00918       erouter->postEvent(EventBase::motmanEGID,mcid,EventBase::deactivateETID,0);
00919     } catch(const std::exception& ex) {
00920       ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00921     } catch(...) {
00922       ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00923     }
00924   }
00925 #ifdef PLATFORM_APERIOS
00926   MotionManagerMsg dmsg;
00927   dmsg.setDelete(mcid);
00928   //cout << "Remove at " << get_time() << flush;
00929   subjs[getAccID()]->SetData(&dmsg,sizeof(dmsg));
00930   subjs[getAccID()]->NotifyObservers();
00931 #else //PLATFORM_LOCAL
00932   // local will "echo" the message, so we'll do the actual remove when we get the echo
00933   SharedObject<MotionManagerMsg> dmsg;
00934   dmsg->setDelete(mcid);
00935   //cout << "Remove at " << get_time() << flush;
00936   try {
00937     subjs[getAccID()]->sendMessage(dmsg.getRegion());
00938   } catch(const std::exception& ex) {
00939     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",&ex);
00940   } catch(...) {
00941     ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",NULL);
00942   }
00943 #endif //PLATFORM check for IPC stuff
00944   func_end();
00945 }
00946 
00947 
00948 /*! Note that we don't actually set the PIDs in the system here, we just queue them up.
00949  *  PID changes seem to be an expensive operation, so may only want to clear the queue
00950  *  at some reduced rate (although that's not actually currently being done, it just
00951  *  could be) */
00952 void
00953 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00954   func_begin();
00955 
00956   //see if there's already an update for this joint
00957   for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00958     if(pidchanges[u].joint==joint) { //found it
00959       for(uint i=0; i<3; i++) {
00960         pidchanges[u].pids[i]=pids[i];
00961         if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID
00962           for(i++; i<3; i++) //we aren't, copy over the rest
00963             pidchanges[u].pids[i]=pids[i];
00964           func_end();
00965           return;
00966         }
00967       }
00968       //if it didn't return within the loop, no difference was found from current state
00969       //so just delete the update
00970       pidchanges.erase(u);
00971       func_end();
00972       return;
00973     }
00974   }
00975 
00976   //if we completed the for loop, we didn't find an update for the joint
00977   for(uint i=0; i<3; i++) //check to see if it's different from the current
00978     if(pids[i]!=state->pids[joint][i]) {
00979       PIDUpdate update(joint,pids); //it is different, insert a new update
00980       pidchanges.push_back(update);
00981       // or for debugging:
00982       //ListMemBuf<PIDUpdate,NumPIDJoints>::index_t it=pidchanges.push_back(update);
00983       //ASSERT(it!=pidchanges.end(),"MotionManager ran out of pidchanges entries!");
00984       break;
00985     }
00986   func_end();
00987 }
00988 
00989 
00990 MotionManager::MC_ID
00991 MotionManager::skip_ahead(MC_ID mcid) const {
00992   // this is in case a new motion has been added, but the current
00993   // process hasn't received its own copy yet, so should skip over them
00994   while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[getAccID()]==NULL)
00995     mcid=cmdlist.next(mcid);
00996   return mcid;
00997 }
00998 
00999 MotionManager::OutputState::OutputState()
01000   : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
01001 {}
01002 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
01003   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01004 {
01005   for(unsigned int i=0; i<NumFrames; i++)
01006     frames[i]=cmds[i];
01007 }
01008 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
01009   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01010 {
01011   for(unsigned int i=0; i<NumFrames; i++)
01012     frames[i]=cmd;
01013 }
01014 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
01015   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01016 {
01017   frames[frame]=cmd;
01018 }
01019 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputPID& p)
01020   : priority(pri),mcid(mc), pid(p)
01021 {}
01022 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
01023   : priority(pri),mcid(mc), pid(p)
01024 {
01025   for(unsigned int i=0; i<NumFrames; i++)
01026     frames[i]=cmds[i];
01027 }
01028 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
01029   : priority(pri),mcid(mc), pid(p)
01030 {
01031   for(unsigned int i=0; i<NumFrames; i++)
01032     frames[i]=cmd;
01033 }
01034 
01035 
01036 /*! @file
01037  * @brief Implements MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
01038  * @author ejt (Creator)
01039  *
01040  * $Author: kcomer $
01041  * $Name: tekkotsu-4_0 $
01042  * $Revision: 1.66 $
01043  * $State: Exp $
01044  * $Date: 2007/11/15 21:33:05 $
01045  */
01046 
01047 
01048 /*
01049     for(uint f=0;f<NumFrames;f++)
01050       for(uint i=0; i<NumOutputs; i++)
01051         outputs[f][i]=0;
01052     const uint cyctime=128;
01053     uint ot=get_time()+3*cyctime;
01054     for(uint f=0;f<NumFrames;f++) {
01055       uint t=ot+f*FrameTime;
01056       outputs[f][TopBrLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01057       t-=cyctime;
01058       outputs[f][TopLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01059       outputs[f][TopRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01060       t-=cyctime;
01061       outputs[f][MidLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01062       outputs[f][MidRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01063       t-=cyctime;
01064       outputs[f][BotLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01065       outputs[f][BotRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
01066     }
01067 */
01068 
01069   /*  for(uint output=TlRedLEDOffset-1; output<LEDOffset+NumLEDs-1; output++) {
01070     cmdstatelist_t& curstatelist=cmdstates[output];
01071     cout << "Out " << output << ": ";
01072     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
01073       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
01074     cout << endl;
01075     }*/
01076 
01077 
01078   /*  cout << get_time() << ' ' << size() << endl;
01079   for(uint output=TlRedLEDOffset; output<LEDOffset+NumLEDs-1; output++) {
01080     cmdstatelist_t& curstatelist=cmdstates[output];
01081     cout << "Out " << output << ": ";
01082     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
01083       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
01084     cout << endl;
01085   }
01086   */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:54 2007 by Doxygen 1.5.4