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

Tekkotsu v2.1
Generated Tue Mar 16 23:19:14 2004 by Doxygen 1.3.5