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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4