Homepage Demos Overview Downloads Tutorials 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 
00006 #include "Shared/ERS210Info.h"
00007 #include "Shared/ERS220Info.h"
00008 #include "Shared/ERS7Info.h"
00009 
00010 #include <list>
00011 
00012 MotionManager * motman=NULL;
00013 int MotionManager::_MMaccID=-1U;
00014 
00015 const float MotionManager::kIgnoredPriority    =-1;
00016 const float MotionManager::kBackgroundPriority = 0;
00017 const float MotionManager::kLowPriority        = 5;
00018 const float MotionManager::kStdPriority        = 10;
00019 const float MotionManager::kHighPriority       = 50;
00020 const float MotionManager::kEmergencyPriority  = 100;
00021 
00022 using namespace std;
00023 
00024 //! just for convenience
00025 typedef unsigned int uint;
00026 
00027 MotionManager::MotionManager()
00028   : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock()
00029 #ifdef PLATFORM_APERIOS
00030   ,numAcc(0)
00031 #endif
00032 {
00033   for(uint x=0; x<NumOutputs; x++)
00034     cmdSums[x]=0;
00035 }
00036 
00037 #ifdef PLATFORM_APERIOS
00038 
00039 void
00040 MotionManager::InitAccess(OSubject* subj) {
00041   if(numAcc==MAX_ACCESS) {
00042     printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00043     return;
00044   }
00045   _MMaccID=numAcc++;
00046   //  cout << "ID is now " << _MMaccID << " of " << numAcc << endl;
00047   //  cout << "_MMaccID is " << &_MMaccID << endl;
00048   //  cout << "numAcc is " << &numAcc << " from " << this << endl;
00049   MMlock.lock(_MMaccID);
00050   //  accRegs[accID].init();
00051   subjs[_MMaccID]=subj;
00052   if(cmdlist.size()>0) //Shouldn't happen - busy wait in addMotion
00053     cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00054   MMlock.release();
00055 }
00056 
00057 #endif //PLATFORM_APERIOS
00058 
00059 void
00060 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
00061   if(output >= NumOutputs)
00062     return;
00063   if(cmd.weight<=0)
00064     return;
00065 
00066   if(caller==NULL || caller->getID()!=cur_cmd)
00067     func_begin();
00068   if(cur_cmd==invalid_MC_ID) {
00069     cmdSums[output]=cmd.value;
00070   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00071     cmdstatelist_t& curstatelist=cmdstates[output];
00072     cmdstatelist_t::index_t ent=curstatelist.begin();
00073     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00074       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
00075     else
00076       for(unsigned int i=0; i<NumFrames; i++)
00077         curstatelist[ent].frames[i]=cmd;
00078   }
00079   if(caller==NULL || caller->getID()!=cur_cmd)
00080     func_end();
00081 }
00082 
00083 void
00084 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
00085   if(output >= NumOutputs)
00086     return;
00087   if(cmd.weight<=0)
00088     return;
00089 
00090   if(caller==NULL || caller->getID()!=cur_cmd)
00091     func_begin();
00092   if(cur_cmd==invalid_MC_ID) {
00093     cmdSums[output]=cmd.value;
00094   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00095     cmdstatelist_t& curstatelist=cmdstates[output];
00096     cmdstatelist_t::index_t ent=curstatelist.begin();
00097     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00098       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
00099     else
00100       curstatelist[ent].frames[frame]=cmd;
00101   }
00102   if(caller==NULL || caller->getID()!=cur_cmd)
00103     func_end();
00104 }
00105 
00106 void 
00107 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
00108   if(output >= NumOutputs)
00109     return;
00110   unsigned int hasWeight=NumFrames;
00111   for(unsigned int i=NumFrames-1; i!=0; i--)
00112     if(ocmds[i].weight>0) {
00113       hasWeight=i;
00114       break;
00115     }
00116   if(hasWeight==NumFrames)
00117     return;
00118   
00119 
00120   if(caller==NULL || caller->getID()!=cur_cmd)
00121     func_begin();
00122   if(cur_cmd==invalid_MC_ID) {
00123     cmdSums[output]=ocmds[hasWeight].value;
00124   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00125     cmdstatelist_t& curstatelist=cmdstates[output];
00126     cmdstatelist_t::index_t ent=curstatelist.begin();
00127     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00128       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
00129     else
00130       for(unsigned int i=0; i<NumFrames; i++)
00131         curstatelist[ent].frames[i]=ocmds[i];
00132   }
00133   if(caller==NULL || caller->getID()!=cur_cmd)
00134     func_end();
00135 }
00136 
00137 void
00138 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
00139   if(output >= NumOutputs)
00140     return;
00141 
00142   if(caller==NULL || caller->getID()!=cur_cmd)
00143     func_begin();
00144   if(cur_cmd==invalid_MC_ID) {
00145     setPID(output,pid.pid);
00146   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00147     cmdstatelist_t& curstatelist=cmdstates[output];
00148     cmdstatelist_t::index_t ent=curstatelist.begin();
00149     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00150       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
00151     else
00152       curstatelist[ent].pid=pid;
00153   }
00154   if(caller==NULL || caller->getID()!=cur_cmd)
00155     func_end();
00156 }
00157 
00158 void
00159 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
00160   if(output >= NumOutputs)
00161     return;
00162 
00163   if(caller==NULL || caller->getID()!=cur_cmd)
00164     func_begin();
00165   if(cur_cmd==invalid_MC_ID) {
00166     if(cmd.weight>0)
00167       cmdSums[output]=cmd.value;
00168     setPID(output,pid.pid);
00169   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00170     cmdstatelist_t& curstatelist=cmdstates[output];
00171     cmdstatelist_t::index_t ent=curstatelist.begin();
00172     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00173       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
00174     else {
00175       for(unsigned int i=0; i<NumFrames; i++)
00176         curstatelist[ent].frames[i]=cmd;
00177       curstatelist[ent].pid=pid;
00178     }
00179   }
00180   if(caller==NULL || caller->getID()!=cur_cmd)
00181     func_end();
00182 }
00183 
00184 void
00185 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
00186   if(output >= NumOutputs)
00187     return;
00188 
00189   if(caller==NULL || caller->getID()!=cur_cmd)
00190     func_begin();
00191   if(cur_cmd==invalid_MC_ID) {
00192     if(ocmds[NumFrames-1].weight>0)
00193       cmdSums[output]=ocmds[NumFrames-1].value;
00194     setPID(output,pid.pid);
00195   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00196     cmdstatelist_t& curstatelist=cmdstates[output];
00197     cmdstatelist_t::index_t ent=curstatelist.begin();
00198     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00199       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds,pid));
00200     else {
00201       for(unsigned int i=0; i<NumFrames; i++)
00202         curstatelist[ent].frames[i]=ocmds[i];
00203       curstatelist[ent].pid=pid;
00204     }
00205   }
00206   if(caller==NULL || caller->getID()!=cur_cmd)
00207     func_end();
00208 }
00209 
00210 /*! What's worse? A plethora of functions which are only called, and only useful at one place,
00211  *  or a big massive function which doesn't pollute the namespace?  This is the latter, for
00212  *  better or worse. */
00213 void
00214 MotionManager::getOutputs(float outputs[NumFrames][NumOutputs]) {
00215   //  if(begin(id)!=end())
00216   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles..." << flush;
00217   if(state==NULL) {
00218     // we haven't gotten the WorldState memory region from Main yet, just set LEDs to a wierd pattern and leave
00219     for(uint f=0;f<NumFrames;f++)
00220       for(uint i=0; i<NumOutputs; i++)
00221         outputs[f][i]=0;
00222     for(uint f=0;f<NumFrames;f++)
00223       for(uint l=0; l<NumLEDs; l++)
00224         outputs[f][l]=l/(NumLEDs-1.0);
00225     //  if(begin(id)!=end())
00226     //if(state && state->buttons[LFrPawOffset]) cout << "getangles-nostate-done..." << flush;
00227     return;
00228   }
00229   func_begin();
00230   //  if(begin(id)!=end())
00231   //  cout << id << "..." << flush;
00232   //  cout << "CHECKOUT..." << flush;
00233   for(uint output=0; output<NumOutputs; output++)
00234     cmdstates[output].clear();
00235 
00236   // for each PID joint which is set to 0 power, set the background
00237   // position value to current sensed value this prevents jerking back
00238   // to the previous position when joint(s) are moved during 0 power,
00239   // and then power is turned back on. (power here is in the 0 pid
00240   // sense, the joints are still receiving power from the system -
00241   // that's a separate system call)
00242   // Note that we wouldn't want to do this all the time, because
00243   // miscalibration between the sensed position and target position
00244   // will cause joints to drift to the extremities of motion, in some
00245   // cases, very quickly, and in worse cases, colliding with other
00246   // joints
00247   for(uint output=0; output<NumPIDJoints; output++)
00248     if(state->pids[output][0]==0 && state->pids[output][1]==0 && state->pids[output][2]==0)
00249       cmdSums[output]=state->outputs[output];
00250 
00251   //  std::cout << "UPDATE..." << std::flush;
00252   std::list<MC_ID> unlocked;
00253   for(MC_ID it=begin(); it!=end(); it=next(it)) // check out all the MotionCommands (only one at a time tho)
00254     unlocked.push_back(it);
00255   while(unlocked.size()>0) { // keep cycling through all the locks we didn't get
00256     for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
00257       MotionCommand* mc=checkoutMotion(*it,false);
00258       if(mc==NULL)
00259         it++; //we didn't get a lock, skip it (we'll keep looping until we get it)
00260       else {
00261         // we got a lock
00262         cur_cmd=*it;
00263         if(mc->shouldPrune()) {
00264           cout << "Removing expired " << *it << " (autoprune)" << endl;
00265           removeMotion(*it);
00266         } else
00267           mc->updateOutputs(); // the MotionCommand should make calls to setOutput from within here
00268         checkinMotion(*it); // release lock, done with motion
00269         // remove id from list of unprocessed motioncommands
00270         std::list<MC_ID>::iterator rem=it++;
00271         unlocked.erase(rem);
00272       }
00273     }
00274     cur_cmd=invalid_MC_ID;
00275   }
00276 
00277   // sort the list of requested outputs based on priority
00278   // (insertion sort, data structure is linked list)
00279   for(uint output=0; output<NumOutputs; output++) {
00280     cmdstatelist_t& curstatelist=cmdstates[output];
00281     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
00282       MC_ID high_ent=bit;
00283       float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
00284       for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
00285         float curp=cmdlist[curstatelist[cit].mcid].priority;
00286         if(curp>high_p) {
00287           high_p=curp;
00288           high_ent=cit;
00289         }
00290       }
00291       curstatelist.swap(bit,high_ent);
00292       /*if(curstatelist.countf()!=curstatelist.countb() || curstatelist.countf()!=curstatelist.size()) {
00293         cout << "LOST ONE! " << bit << ' ' << high_ent << endl;
00294         cout << curstatelist.countf() << ' ' << curstatelist.countb() << ' ' << curstatelist.size() << endl;
00295         }*/
00296       bit=high_ent;
00297     }
00298   }
00299 
00300   // now we've got, for each output, a list of requested values sorted by priority
00301   // summarize each output
00302   for(uint frame=0; frame<NumFrames; frame++)
00303     for(uint output=0; output<NumOutputs; output++) {
00304       cmdstatelist_t& curstatelist=cmdstates[output];
00305       float alpha=1;
00306       OutputCmd sumcmd;
00307       cmdstatelist_t::index_t ent=curstatelist.begin();
00308       while(ent!=curstatelist.end() && alpha>0) {
00309         OutputCmd curcmd;
00310         float curp=curstatelist[ent].priority;
00311         float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00312         for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00313           //weighted average within priority level
00314           float curweight=curstatelist[ent].frames[frame].weight;
00315           ASSERT(curweight>=0,"negative output weights are illegal");
00316           if(curweight<0) { //negative weights are illegal
00317             cout << "weight=" << curweight << endl;
00318             curweight=0;
00319           }
00320           curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
00321           curcmd.weight+=curweight;
00322           if(curweight<1)
00323             curalpha*=(1-curweight);
00324           else
00325             curalpha=0;
00326         }
00327         if(curcmd.weight>0) {
00328           //weighted average of priority levels
00329           sumcmd.value+=curcmd.value/curcmd.weight*(1-curalpha);
00330           sumcmd.weight+=(1-curalpha);
00331           alpha*=curalpha;
00332         }
00333       }
00334       if(sumcmd.weight>0) 
00335         outputs[frame][output]=sumcmd.value/sumcmd.weight;
00336       else //if zero weight, hold last value
00337         outputs[frame][output]=cmdSums[output];
00338       if(frame==NumFrames-1)
00339         cmds[output]=sumcmd;
00340     }
00341   
00342   for(uint output=0; output<NumOutputs; output++)
00343     cmdSums[output]=outputs[NumFrames-1][output];
00344         
00345   // now summarize each output's PID values (for those which use PID control)
00346   for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
00347     cmdstatelist_t& curstatelist=cmdstates[output];
00348     float alpha=1;
00349     float sumpid[3];
00350     for(uint i=0; i<3; i++)
00351       sumpid[i]=0;
00352     float sumweight=0;
00353     cmdstatelist_t::index_t ent=curstatelist.begin();
00354     while(ent!=curstatelist.end() && alpha>0) {
00355       float tmppid[3];
00356       for(uint i=0; i<3; i++)
00357         tmppid[i]=0;
00358       float tmpweight=0;
00359       float curp=curstatelist[ent].priority;
00360       float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00361       for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00362         //weighted average within priority level
00363         float curweight=curstatelist[ent].pid.weight;
00364         ASSERT(curweight>=0,"negative PID weights are illegal")
00365         if(curweight<0) //negative weights are illegal
00366           curweight=0;
00367         for(uint i=0; i<3; i++)
00368           tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
00369         tmpweight+=curweight;
00370         if(curweight<1)
00371           curalpha*=(1-curweight);
00372         else
00373           curalpha=0;
00374       }
00375       if(tmpweight>0) {
00376         //weighted average of priority levels
00377         for(uint i=0; i<3; i++)
00378           sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
00379         sumweight+=(1-curalpha);
00380         alpha*=curalpha;
00381       }
00382     }
00383     if(sumweight>0) {
00384       for(uint i=0; i<3; i++)
00385         sumpid[i]/=sumweight;
00386       setPID(output,sumpid);
00387     }
00388   }
00389 
00390   func_end();
00391   //  if(begin(id)!=end())
00392   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles-done." << flush;
00393 }
00394 
00395 void
00396 MotionManager::updateWorldState() {
00397   for(uint output=LEDOffset; output<LEDOffset+NumLEDs; output++)
00398     state->outputs[output]=cmdSums[output];
00399   for(uint output=BinJointOffset; output<BinJointOffset+NumBinJoints; output++)
00400     state->outputs[output]=cmdSums[output];
00401 
00402   // these parts check to see if there are "fake" joints, and sets their "sensed" values
00403   // to be the current target value, just in case a behavior is waiting for a non-existant
00404   // non-existant joint to move to a certain position.
00405   if(state->robotDesign & WorldState::ERS210Mask) {
00406     for(uint output=0; output<NumPIDJoints; output++)
00407       if(!ERS210Info::IsRealERS210[output])
00408         state->outputs[output]=cmdSums[output];
00409   } else if(state->robotDesign & WorldState::ERS220Mask) {
00410     for(uint output=0; output<NumPIDJoints; output++)
00411       if(!ERS220Info::IsRealERS220[output])
00412         state->outputs[output]=cmdSums[output];
00413   } else if(state->robotDesign & WorldState::ERS7Mask) {
00414     for(uint output=0; output<NumPIDJoints; output++)
00415       if(!ERS7Info::IsRealERS7[output])
00416         state->outputs[output]=cmdSums[output];
00417   } else
00418     cout << "MotionManager::updateWorldState() - could not detect model" << endl;
00419 }
00420 
00421 #ifdef PLATFORM_APERIOS
00422 
00423 
00424 /*! This function handles the conversion from the Tekkotsu format (one
00425  *  regular IEEE float per parameter, to the OPEN-R format (which
00426  *  takes a specialized, reduced precision floating point number) This
00427  *  is all documented in PIDMC as well.
00428  *
00429  *  In order to send Tekkotsu's PIDs to the system, they are converted
00430  *  to the gain/shift format.  On the ERS-2xx, we could dynamically
00431  *  choose shift values to allow more precision in setting values.
00432  *
00433  *  With the ERS-7, all shifts are shared, so they must be set to a
00434  *  common set of values, defined by WorldState::DefaultPIDShifts.
00435  *  This limits the range of gains which can then be set.
00436  *
00437  *  Due to the mysterious warning which would occur with the 2xx,
00438  *  (AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.)  and
00439  *  since this seems to be the way things are going, all models now,
00440  *  by default, use global shift values (which can vary from model to
00441  *  model, just global for each model)
00442  *
00443  *  You can revert to the dynamic shift selection by commenting-in
00444  *  the noted code section below.
00445  *
00446  *  A final note: the OPENR::SetJointGain function seems to be
00447  *  a rather costly function call.  You should probably try to avoid
00448  *  setting PIDs at too high a frequency.
00449  */
00450 bool
00451 MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
00452   bool dirty=!pidchanges.empty();
00453   while(!pidchanges.empty()) {
00454     float gain[3];
00455     word shift[3];
00456 
00457     //if you want to enforce the default shifts:
00458     for(uint i=0; i<3; i++) {
00459       shift[i]=DefaultPIDShifts[i];
00460       gain[i]=pidchanges.front().pids[i]*(1<<(0x10-shift[i]));
00461     }
00462 
00463     //if you want to allow shifts to move for better precision:
00464     // this is OK on 2xx, although it occasionally produces warnings like:
00465     // AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.
00466     // It still seems to work fine though.
00467     // 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 )
00468     // all joints share the same shift, and so there must be one
00469     // global setting enforced.  If this is the way things are heading,
00470     // might as well just have everyone use the first method instead.
00471     // but here's the more dynamic way just for posterity:
00472     /*
00473     for(uint i=0; i<3; i++) {
00474       gain[i]=pidchanges.front().pids[i]*2;
00475       shift[i]=0xF;
00476       while(shift[i]!=2 && (gain[i]!=(word)gain[i] || gain[i]<=1) && gain[i]<0x20) {
00477         gain[i]*=2;
00478         shift[i]--;
00479       }
00480     }
00481     */
00482 
00483     //some debugging output (pick your favorite joint - i was having trouble with the ERS-7 nod joint in particular)
00484     //if(pidchanges.front().joint==HeadOffset+NodOffset)
00485     //cout << (word)gain[0] << ' ' << shift[0] << "   " << (word)gain[1] << ' ' << shift[1] << "   " << (word)gain[2] << ' ' << shift[2] << endl;
00486     //cout << gain[0] << ' ' << shift[0] << "   " << gain[1] << ' ' << shift[1] << "   " << gain[2] << ' ' << shift[2] << endl;
00487 
00488     OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
00489     for(uint i=0; i<3; i++)
00490       state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
00491     pidchanges.pop_front();
00492   }
00493   return dirty;
00494 }
00495 
00496 /*! @deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises */
00497 MotionManager::MC_ID
00498 MotionManager::addMotion(const SharedObjectBase& sm) {
00499   return doAddMotion(sm,true,kStdPriority);
00500 }
00501 /*! @deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises */
00502 MotionManager::MC_ID 
00503 MotionManager::addMotion(const SharedObjectBase& sm, bool autoprune) {
00504   return doAddMotion(sm,autoprune,kStdPriority);
00505 }
00506 /*! @deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises */
00507 MotionManager::MC_ID
00508 MotionManager::addMotion(const SharedObjectBase& sm, float priority) {
00509   return doAddMotion(sm,true,priority);
00510 }
00511 /*! @deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises */
00512 MotionManager::MC_ID 
00513 MotionManager::addMotion(const SharedObjectBase& sm, float priority, bool autoprune) {
00514   return doAddMotion(sm,autoprune,priority);
00515 }
00516 /*! We have made this function protected because it's more readable if you
00517  *  call addPrunableMotion() or addPersistentMotion() instead... we decided
00518  *  requiring people to pass a true/false arguement wouldn't make it clear
00519  *  what that true/false was controlling. */
00520 MotionManager::MC_ID 
00521 MotionManager::doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority) {
00522   MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
00523   if(mc==NULL) {
00524     cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
00525     return invalid_MC_ID;
00526   }
00527   mc->setAutoPrune(autoprune);
00528   //cout << "addMotion...";
00529   while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; } //Wait for everyone to register
00530   func_begin();
00531   //cout << cmdlist.size() << " exist..." << endl;
00532   //  cout << id << "..." << flush;
00533   MC_ID mc_id = pop_free();
00534   if(mc_id==cmdlist.end()) {
00535     cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
00536     return func_end(cmdlist.end());
00537   }
00538   cmdlist[mc_id].baseaddrs[_MMaccID]=mc;
00539   cmdlist[mc_id].rcr[_MMaccID]=sm.getRegion();
00540   //cout << "addMotion(): sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00541   cmdlist[mc_id].rcr[_MMaccID]->AddReference();
00542   //cout << "addMotion()NOW: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00543   cmdlist[mc_id].lastAccessor=_MMaccID;
00544   cmdlist[mc_id].priority=priority;
00545   mc->setAdd(mc_id);
00546   OStatus err;
00547   /*{
00548     unsigned int i=0;
00549     for(ObserverConstIterator it=subjs[_MMaccID]->begin();it!=subjs[_MMaccID]->end();it++) {
00550       cout << "RemainBuffer("<<i++<<")==" << subjs[_MMaccID]->RemainBuffer(*it) << endl;
00551     }
00552     ASSERT((int)i==subjs[_MMaccID]->NumberOfObservers(),"did I miss an observer?");
00553   }*/
00554   ASSERT((err=subjs[_MMaccID]->SetData(sm.getRegion()))==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00555   //cout << "addMotion()afterSetData: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00556   ASSERT((err=subjs[_MMaccID]->NotifyObservers())==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00557   //  cout << "addMotion-done" << endl;
00558   return func_end(mc_id);
00559 }
00560 
00561 void
00562 MotionManager::receivedMsg(const ONotifyEvent& event) {
00563   //  cout << "receivedMsg..." << flush;
00564   func_begin();
00565   //  cout << id << "..." << flush;
00566   for(int x=0; x<event.NumOfData(); x++) {
00567     RCRegion * rcr = event.RCData(x);
00568     MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00569     MC_ID mc_id=mminfo->mc_id;
00570     switch(mminfo->type) {
00571     case MotionManagerMsg::addMotion: {
00572       //cout << "receiveMotion(): rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00573       rcr->AddReference();
00574       //cout << "receiveMotion()NOW: rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00575       cmdlist[mc_id].rcr[_MMaccID]=rcr;
00576       //should be able to do a nice dynamic cast instead of a static one
00577       // but it gives NULL for some reason - i blame having to do the fork trick
00578       cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast<MotionCommand*>(mminfo);
00579       erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,0));
00580       cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart();
00581     } break;
00582     case MotionManagerMsg::deleteMotion: {
00583       //cout << "deleteMotion(): cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
00584       cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00585       //cout << "deleteMotion()NOW: cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
00586     } break;
00587     default:
00588       printf("*** WARNING *** unknown MotionManager msg type received\n");
00589     }
00590   }
00591   //  cout << "receivedMsg-done" << endl;
00592   func_end();
00593 }
00594 
00595 #endif //PLATFORM_APERIOS
00596 
00597 MotionCommand *
00598 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00599   //cout << "checkout..." << flush;
00600   if(mcid>=MAX_MOTIONS) {
00601     cout << "*** WARNING *** " << _MMaccID << " tried to access invalid mcid " << mcid << endl;
00602     return NULL;
00603   }
00604   if(block)
00605     cmdlist[mcid].lock.lock(_MMaccID);
00606   else
00607     if(!cmdlist[mcid].lock.try_lock(_MMaccID))
00608       return NULL;
00609   if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00610     cout << "*** WARNING *** " << _MMaccID << " tried to access dead mcid " << mcid << endl;
00611     cmdlist[mcid].lock.release();
00612     return NULL;
00613   }
00614   //cout << "locked..." << endl;
00615   MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
00616   //  cout << "base=" << base << "..." << flush;
00617   if(cmdlist[mcid].lastAccessor!=_MMaccID) {
00618     //cout << "converting from " << MCRegistrar::getRaw(base) << "..." << flush;
00619     //cout << "prev=" << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << "..." << flush;
00620     //    accRegs[id].convert(base);
00621     //cout << "to=" << MCRegistrar::getRaw(base) << ", " << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << endl;
00622     cmdlist[mcid].lastAccessor=_MMaccID;
00623   }
00624   //cout << "checkout-done..." << flush;
00625   return base;
00626 }
00627 
00628 void
00629 MotionManager::checkinMotion(MC_ID mcid) {
00630   if(mcid!=invalid_MC_ID)
00631     cmdlist[mcid].lock.release();
00632 }
00633 
00634 void
00635 MotionManager::removeMotion(MC_ID mcid) {
00636   if(mcid==invalid_MC_ID)
00637     return;
00638   func_begin();
00639   checkoutMotion(mcid,true);
00640   cmdlist[mcid].baseaddrs[_MMaccID]->DoStop();
00641   erouter->postEvent(new EventBase(EventBase::motmanEGID,mcid,EventBase::deactivateETID,0));
00642 #ifdef PLATFORM_APERIOS
00643   MotionManagerMsg dmsg;
00644   dmsg.setDelete(mcid);
00645   subjs[_MMaccID]->SetData(&dmsg,sizeof(dmsg));
00646   subjs[_MMaccID]->NotifyObservers();
00647   //cout << "removeMotion(): cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
00648   cmdlist[mcid].rcr[_MMaccID]->RemoveReference();
00649   //cout << "removeMotion()NOW: cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
00650 #endif //PLATFORM_APERIOS
00651   push_free(mcid);
00652   checkinMotion(mcid);
00653   func_end();
00654 }
00655 
00656 
00657 /*! Note that we don't actually set the PIDs in the system here, we just queue them up.
00658  *  PID changes seem to be an expensive operation, so may only want to clear the queue
00659  *  at some reduced rate (although that's not actually currently being done, it just
00660  *  could be) */
00661 void
00662 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00663   func_begin();
00664 
00665   //see if there's already an update for this joint
00666   for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00667     if(pidchanges[u].joint==joint) { //found it
00668       for(uint i=0; i<3; i++) {
00669         pidchanges[u].pids[i]=pids[i];
00670         if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID
00671           for(i++; i<3; i++) //we aren't, copy over the rest
00672             pidchanges[u].pids[i]=pids[i];
00673           func_end();
00674           return;
00675         }
00676       }
00677       //if it didn't return within the loop, no difference was found from current state
00678       //so just delete the update
00679       pidchanges.erase(u);
00680       func_end();
00681       return;
00682     }
00683   }
00684 
00685   //if we completed the for loop, we didn't find an update for the joint
00686   for(uint i=0; i<3; i++) //check to see if it's different from the current
00687     if(pids[i]!=state->pids[joint][i]) {
00688       PIDUpdate update(joint,pids); //it is different, insert a new update
00689       pidchanges.push_back(update);
00690       break;
00691     }
00692   func_end();
00693 }
00694 
00695 
00696 MotionManager::MC_ID
00697 MotionManager::skip_ahead(MC_ID mcid) const {
00698   // this is in case a new motion has been added, but the current
00699   // process hasn't received its own copy yet, so should skip over them
00700 #ifdef PLATFORM_APERIOS
00701   while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[_MMaccID]==NULL)
00702     mcid=cmdlist.next(mcid);
00703   return mcid;
00704 #else
00705   return cmdlist.next(mcid);
00706 #endif
00707 }
00708 
00709 MotionManager::OutputState::OutputState()
00710   : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
00711 {}
00712 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
00713   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00714 {
00715   for(unsigned int i=0; i<NumFrames; i++)
00716     frames[i]=cmds[i];
00717 }
00718 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
00719   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00720 {
00721   for(unsigned int i=0; i<NumFrames; i++)
00722     frames[i]=cmd;
00723 }
00724 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
00725   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00726 {
00727   frames[frame]=cmd;
00728 }
00729 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputPID& p)
00730   : priority(pri),mcid(mc), pid(p)
00731 {}
00732 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
00733   : priority(pri),mcid(mc), pid(p)
00734 {
00735   for(unsigned int i=0; i<NumFrames; i++)
00736     frames[i]=cmds[i];
00737 }
00738 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
00739   : priority(pri),mcid(mc), pid(p)
00740 {
00741   for(unsigned int i=0; i<NumFrames; i++)
00742     frames[i]=cmd;
00743 }
00744 
00745 
00746 /*! @file
00747  * @brief Implements MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
00748  * @author ejt (Creator)
00749  *
00750  * $Author: ejt $
00751  * $Name: tekkotsu-2_2 $
00752  * $Revision: 1.38 $
00753  * $State: Exp $
00754  * $Date: 2004/10/17 01:16:11 $
00755  */
00756 
00757 
00758 /*
00759     for(uint f=0;f<NumFrames;f++)
00760       for(uint i=0; i<NumOutputs; i++)
00761         outputs[f][i]=0;
00762     const uint cyctime=128;
00763     uint ot=get_time()+3*cyctime;
00764     for(uint f=0;f<NumFrames;f++) {
00765       uint t=ot+f*FrameTime;
00766       outputs[f][TopBrLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00767       t-=cyctime;
00768       outputs[f][TopLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00769       outputs[f][TopRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00770       t-=cyctime;
00771       outputs[f][MidLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00772       outputs[f][MidRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00773       t-=cyctime;
00774       outputs[f][BotLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00775       outputs[f][BotRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00776     }
00777 */
00778 
00779   /*  for(uint output=TlRedLEDOffset-1; output<LEDOffset+NumLEDs-1; output++) {
00780     cmdstatelist_t& curstatelist=cmdstates[output];
00781     cout << "Out " << output << ": ";
00782     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00783       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00784     cout << endl;
00785     }*/
00786 
00787 
00788   /*  cout << get_time() << ' ' << size() << endl;
00789   for(uint output=TlRedLEDOffset; output<LEDOffset+NumLEDs-1; output++) {
00790     cmdstatelist_t& curstatelist=cmdstates[output];
00791     cout << "Out " << output << ": ";
00792     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00793       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00794     cout << endl;
00795   }
00796   */

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1