Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

MotionManager.cc

Go to the documentation of this file.

#include "MotionManager.h"
#include "Shared/debuget.h"
#include "Shared/WorldState.h"
#include "Events/EventRouter.h"

#include "Shared/ERS210Info.h"
#include "Shared/ERS220Info.h"

#include <list>

MotionManager * motman=NULL;
int MotionManager::_MMaccID=-1U;

const float MotionManager::kIgnoredPriority    =-1;
const float MotionManager::kBackgroundPriority = 0;
const float MotionManager::kLowPriority        = 5;
const float MotionManager::kStdPriority        = 10;
const float MotionManager::kHighPriority       = 50;
const float MotionManager::kEmergencyPriority  = 100;

#ifndef PLATFORM_APERIOS
using std::cout;
using std::endl;
#endif


//! just for convenience
typedef unsigned int uint;

MotionManager::MotionManager()
  : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock()
#ifdef PLATFORM_APERIOS
  ,numAcc(0)
#endif
{
  for(uint x=0; x<NumOutputs; x++)
    cmdSums[x]=0;
}

#ifdef PLATFORM_APERIOS

void
MotionManager::InitAccess(OSubject* subj) {
  if(numAcc==MAX_ACCESS) {
    printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
    return;
  }
  _MMaccID=numAcc++;
  //  cout << "ID is now " << _MMaccID << " of " << numAcc << endl;
  //  cout << "_MMaccID is " << &_MMaccID << endl;
  //  cout << "numAcc is " << &numAcc << " from " << this << endl;
  MMlock.lock(_MMaccID);
  //  accRegs[accID].init();
  subjs[_MMaccID]=subj;
  if(cmdlist.size()>0) //Shouldn't happen - busy wait in addMotion
    cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
  MMlock.release();
}

#endif //PLATFORM_APERIOS

void
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    if(cmd.weight>0)
      cmdSums[output]=cmd.value;
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
    else
      for(unsigned int i=0; i<NumFrames; i++)
        curstatelist[ent].frames[i]=cmd;
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

void
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    if(cmd.weight>0)
      cmdSums[output]=cmd.value;
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
    else
      curstatelist[ent].frames[frame]=cmd;
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

void 
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    if(ocmds[NumFrames-1].weight>0)
      cmdSums[output]=ocmds[NumFrames-1].value;
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
    else
      for(unsigned int i=0; i<NumFrames; i++)
        curstatelist[ent].frames[i]=ocmds[i];
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

void
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    //!@todo should be able to set background pid
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
    else
      curstatelist[ent].pid=pid;
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

void
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    if(cmd.weight>0)
      cmdSums[output]=cmd.value;
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
    else {
      for(unsigned int i=0; i<NumFrames; i++)
        curstatelist[ent].frames[i]=cmd;
      curstatelist[ent].pid=pid;
    }
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

void
MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
    if (output >= NumOutputs) return; 

  if(caller==NULL || caller->getID()!=cur_cmd)
    func_begin();
  if(cur_cmd==invalid_MC_ID) {
    if(ocmds[NumFrames-1].weight>0)
      cmdSums[output]=ocmds[NumFrames-1].value;
  } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cmdstatelist_t::index_t ent=curstatelist.begin();
    if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
      curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
    else {
      for(unsigned int i=0; i<NumFrames; i++)
        curstatelist[ent].frames[i]=ocmds[i];
      curstatelist[ent].pid=pid;
    }
  }
  if(caller==NULL || caller->getID()!=cur_cmd)
    func_end();
}

/*! What's worse? A plethora of functions which are only called, and only useful at one place,
 *  or a big massive function which doesn't pollute the namespace?  This is the latter, for
 *  better or worse. */
void
MotionManager::getOutputs(float outputs[NumFrames][NumOutputs]) {
  //  if(begin(id)!=end())
  //if(state && state->buttons[LFrPawOffset]) cout << "getAngles..." << flush;
  if(state==NULL) {
    // we haven't gotten the WorldState memory region from Main yet, just set LEDs to a wierd pattern and leave
    for(uint f=0;f<NumFrames;f++)
      for(uint i=0; i<NumOutputs; i++)
        outputs[f][i]=0;
    for(uint f=0;f<NumFrames;f++)
      for(uint l=0; l<NumLEDs; l++)
        outputs[f][l]=l/(NumLEDs-1.0);
    //  if(begin(id)!=end())
    //if(state && state->buttons[LFrPawOffset]) cout << "getangles-nostate-done..." << flush;
    return;
  }
  func_begin();
  //  if(begin(id)!=end())
  //  cout << id << "..." << flush;
  //  cout << "CHECKOUT..." << flush;
  for(uint output=0; output<NumOutputs; output++)
    cmdstates[output].clear();

  //  std::cout << "UPDATE..." << std::flush;
  std::list<MC_ID> unlocked;
  for(MC_ID it=begin(); it!=end(); it=next(it)) // check out all the MotionCommands (only one at a time tho)
    unlocked.push_back(it);
  while(unlocked.size()>0) { // keep cycling through all the locks we didn't get
    for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
      MotionCommand* mc=checkoutMotion(*it,false);
      if(mc==NULL)
        it++; //we didn't get a lock, skip it (we'll keep looping until we get it)
      else {
        // we got a lock
        cur_cmd=*it;
        if(mc->shouldPrune()) {
          cout << "Removing expired" << endl;
          removeMotion(*it);
        } else
          mc->updateOutputs(); // the MotionCommand should make calls to setOutput from within here
        checkinMotion(*it); // release lock, done with motion
        // remove id from list of unprocessed motioncommands
        std::list<MC_ID>::iterator rem=it++;
        unlocked.erase(rem);
      }
    }
    cur_cmd=invalid_MC_ID;
  }

  // sort the list of requested outputs based on priority
  // (insertion sort, data structure is linked list)
  for(uint output=0; output<NumOutputs; output++) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
      MC_ID high_ent=bit;
      float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
      for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
        float curp=cmdlist[curstatelist[cit].mcid].priority;
        if(curp>high_p) {
          high_p=curp;
          high_ent=cit;
        }
      }
      curstatelist.swap(bit,high_ent);
      /*if(curstatelist.countf()!=curstatelist.countb() || curstatelist.countf()!=curstatelist.size()) {
        cout << "LOST ONE! " << bit << ' ' << high_ent << endl;
        cout << curstatelist.countf() << ' ' << curstatelist.countb() << ' ' << curstatelist.size() << endl;
        }*/
      bit=high_ent;
    }
  }

  // now we've got, for each output, a list of requested values sorted by priority
  // summarize each output
  for(uint frame=0; frame<NumFrames; frame++)
    for(uint output=0; output<NumOutputs; output++) {
      cmdstatelist_t& curstatelist=cmdstates[output];
      float alpha=1;
      OutputCmd sumcmd;
      cmdstatelist_t::index_t ent=curstatelist.begin();
      while(ent!=curstatelist.end() && alpha>0) {
        OutputCmd curcmd;
        float curp=curstatelist[ent].priority;
        float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
        for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
          //weighted average within priority level
          float curweight=curstatelist[ent].frames[frame].weight;
          ASSERT(curweight>=0,"negative output weights are illegal");
          if(curweight<0) { //negative weights are illegal
            cout << "weight=" << curweight << endl;
            curweight=0;
          }
          curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
          curcmd.weight+=curweight;
          if(curweight<1)
            curalpha*=(1-curweight);
          else
            curalpha=0;
        }
        if(curcmd.weight>0) {
          //weighted average of priority levels
          sumcmd.value+=curcmd.value/curcmd.weight*(1-curalpha);
          sumcmd.weight+=(1-curalpha);
          alpha*=curalpha;
        }
      }
      if(sumcmd.weight>0) 
        outputs[frame][output]=sumcmd.value/sumcmd.weight;
      else //if zero weight, hold last value
        outputs[frame][output]=cmdSums[output];
      if(frame==NumFrames-1)
        cmds[output]=sumcmd;
    }
  
  for(uint output=0; output<NumOutputs; output++)
    cmdSums[output]=outputs[NumFrames-1][output];
        
  // now summarize each output's PID values (for those which use PID control)
  for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    float alpha=1;
    float sumpid[3];
    for(uint i=0; i<3; i++)
      sumpid[i]=0;
    float sumweight=0;
    cmdstatelist_t::index_t ent=curstatelist.begin();
    while(ent!=curstatelist.end() && alpha>0) {
      float tmppid[3];
      for(uint i=0; i<3; i++)
        tmppid[i]=0;
      float tmpweight=0;
      float curp=curstatelist[ent].priority;
      float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
      for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
        //weighted average within priority level
        float curweight=curstatelist[ent].pid.weight;
        ASSERT(curweight>=0,"negative PID weights are illegal")
        if(curweight<0) //negative weights are illegal
          curweight=0;
        for(uint i=0; i<3; i++)
          tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
        tmpweight+=curweight;
        if(curweight<1)
          curalpha*=(1-curweight);
        else
          curalpha=0;
      }
      if(tmpweight>0) {
        //weighted average of priority levels
        for(uint i=0; i<3; i++)
          sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
        sumweight+=(1-curalpha);
        alpha*=curalpha;
      }
    }
    if(sumweight>0) {
      for(uint i=0; i<3; i++)
        sumpid[i]/=sumweight;
      setPID(output,sumpid);
    }
  }

  func_end();
  //  if(begin(id)!=end())
  //if(state && state->buttons[LFrPawOffset]) cout << "getAngles-done." << flush;
}

void
MotionManager::updateWorldState() {
  for(uint output=LEDOffset; output<LEDOffset+NumLEDs; output++)
    state->outputs[output]=cmdSums[output];
  for(uint output=BinJointOffset; output<BinJointOffset+NumBinJoints; output++)
    state->outputs[output]=cmdSums[output];
  if(state->robotDesign & WorldState::ERS210Mask) {
    for(uint output=0; output<NumPIDJoints; output++)      //NOTE: Should probably be NumOutputs for portability, but this is faster
      if(!ERS210Info::IsRealERS210[output])
        state->outputs[output]=cmdSums[output];
  } else if(state->robotDesign & WorldState::ERS220Mask) {
    for(uint output=0; output<NumPIDJoints; output++)      //NOTE: Should probably be NumOutputs for portability, but this is faster
      if(!ERS220Info::IsRealERS220[output])
        state->outputs[output]=cmdSums[output];
  } else
    cout << "MotionManager::updateWorldState() - could not detect model" << endl;
}

#ifdef PLATFORM_APERIOS

bool
MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
  bool dirty=!pidchanges.empty();
  while(!pidchanges.empty()) {
    float gain[3];
    word shift[3];
    for(uint i=0; i<3; i++) {
      gain[i]=pidchanges.front().pids[i]*2;
      shift[i]=0xF;
      while(shift[i]!=0 && gain[i]!=(int)gain[i] && gain[i]<(1<<15)) {
        gain[i]*=2;
        shift[i]--;
      }
    }
    OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
    for(uint i=0; i<3; i++)
      state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
    pidchanges.pop_front();
  }
  return dirty;
}

// documentation for this function is at the end of the file
MotionManager::MC_ID
MotionManager::addMotion(const SharedObjectBase& sm) {
  //  cout << "addMotion..." << flush;
  while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; } //Wait for everyone to register
  func_begin();
  //  cout << id << "..." << flush;
  MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
  if(mc==NULL) {
    cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
    return invalid_MC_ID;
  }
  MC_ID mc_id = pop_free();
  if(mc_id==cmdlist.end()) {
    cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
    return func_end(cmdlist.end());
  }
  cmdlist[mc_id].baseaddrs[_MMaccID]=mc;
  cmdlist[mc_id].rcr[_MMaccID]=sm.getRegion();
  //cout << "addMotion(): sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
  cmdlist[mc_id].rcr[_MMaccID]->AddReference();
  //cout << "addMotion()NOW: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
  cmdlist[mc_id].lastAccessor=_MMaccID;
  cmdlist[mc_id].priority=kStdPriority;
  mc->setAdd(mc_id);
  OStatus err;
  ASSERT((err=subjs[_MMaccID]->SetData(sm.getRegion()))==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
  //cout << "addMotion()afterSetData: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
  ASSERT((err=subjs[_MMaccID]->NotifyObservers())==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
  //  cout << "addMotion-done" << endl;
  return func_end(mc_id);
}
MotionManager::MC_ID 
MotionManager::addMotion(const SharedObjectBase& sm, float priority) {
  func_begin();
  MC_ID mcid=addMotion(sm);
  if(mcid!=end())
    setPriority(mcid,priority);
  return func_end(mcid);
}
MotionManager::MC_ID 
MotionManager::addMotion(const SharedObjectBase& sm, bool autoprune) {
  MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
  if(mc==NULL) {
    cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
    return invalid_MC_ID;
  }
  mc->setAutoPrune(autoprune);
  return addMotion(sm); 
}
MotionManager::MC_ID 
MotionManager::addMotion(const SharedObjectBase& sm, float priority, bool autoprune) {
  func_begin();
  MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
  if(mc==NULL) {
    cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
    return invalid_MC_ID;
  }
  mc->setAutoPrune(autoprune);
  MC_ID mcid=addMotion(sm);
  if(mcid!=end())
    setPriority(mcid,priority);
  return func_end(mcid);
}

void
MotionManager::receivedMsg(const ONotifyEvent& event) {
  //  cout << "receivedMsg..." << flush;
  func_begin();
  //  cout << id << "..." << flush;
  for(int x=0; x<event.NumOfData(); x++) {
    RCRegion * rcr = event.RCData(x);
    MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
    MC_ID mc_id=mminfo->mc_id;
    switch(mminfo->type) {
    case MotionManagerMsg::addMotion: {
      //cout << "receiveMotion(): rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
      rcr->AddReference();
      //cout << "receiveMotion()NOW: rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
      cmdlist[mc_id].rcr[_MMaccID]=rcr;
      //should be able to do a nice dynamic cast instead of a static one
      // but it gives NULL for some reason - i blame having to do the fork trick
      cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast<MotionCommand*>(mminfo);
      erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,00));
      cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart();
    } break;
    case MotionManagerMsg::deleteMotion: {
      //cout << "deleteMotion(): cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
      cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
      //cout << "deleteMotion()NOW: cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
    } break;
    default:
      printf("*** WARNING *** unknown MotionManager msg type received\n");
    }
  }
  //  cout << "receivedMsg-done" << endl;
  func_end();
}

#endif //PLATFORM_APERIOS

MotionCommand *
MotionManager::checkoutMotion(MC_ID mcid,bool block) {
  //cout << "checkout..." << flush;
  if(mcid>=MAX_MOTIONS) {
    cout << "*** WARNING *** " << _MMaccID << " tried to access invalid mcid " << mcid << endl;
    return NULL;
  }
  if(block)
    cmdlist[mcid].lock.lock(_MMaccID);
  else
    if(!cmdlist[mcid].lock.try_lock(_MMaccID))
      return NULL;
  if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
    cout << "*** WARNING *** " << _MMaccID << " tried to access dead mcid " << mcid << endl;
    cmdlist[mcid].lock.release();
    return NULL;
  }
  //cout << "locked..." << endl;
  MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
  //  cout << "base=" << base << "..." << flush;
  if(cmdlist[mcid].lastAccessor!=_MMaccID) {
    //cout << "converting from " << MCRegistrar::getRaw(base) << "..." << flush;
    //cout << "prev=" << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << "..." << flush;
    //    accRegs[id].convert(base);
    //cout << "to=" << MCRegistrar::getRaw(base) << ", " << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << endl;
    cmdlist[mcid].lastAccessor=_MMaccID;
  }
  //cout << "checkout-done..." << flush;
  return base;
}

void
MotionManager::checkinMotion(MC_ID mcid) {
  if(mcid!=invalid_MC_ID)
    cmdlist[mcid].lock.release();
}

void
MotionManager::removeMotion(MC_ID mcid) {
  if(mcid==invalid_MC_ID)
    return;
  func_begin();
  checkoutMotion(mcid,true);
  cmdlist[mcid].baseaddrs[_MMaccID]->DoStop();
  erouter->postEvent(new EventBase(EventBase::motmanEGID,mcid,EventBase::deactivateETID,00));
#ifdef PLATFORM_APERIOS
  MotionManagerMsg dmsg;
  dmsg.setDelete(mcid);
  subjs[_MMaccID]->SetData(&dmsg,sizeof(dmsg));
  subjs[_MMaccID]->NotifyObservers();
  //cout << "removeMotion(): cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
  cmdlist[mcid].rcr[_MMaccID]->RemoveReference();
  //cout << "removeMotion()NOW: cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
#endif //PLATFORM_APERIOS
  push_free(mcid);
  checkinMotion(mcid);
  func_end();
}


/*! Note that we don't actually set the PIDs in the system here, we just queue them up.
 *  PID changes seem to be an expensive operation, so may only want to clear the queue
 *  at some reduced rate (although that's not actually currently implemented, so right
 *  now there's no real benefit until that's done) */
void
MotionManager::setPID(unsigned int joint, const float pids[3]) {
  func_begin();
  //see if there's already an update for this joint
  for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
    if(pidchanges[u].joint==joint) { //found it
      for(uint i=0; i<3; i++) {
        pidchanges[i].pids[i]=pids[i];
        if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID
          for(i++; i<3; i++) //we aren't, copy over the rest
            pidchanges[i].pids[i]=pids[i];
          func_end();
          return;
        }
      }
      //if it didn't return within the loop, no difference was found from current state
      //so just delete the update
      pidchanges.erase(u);
      func_end();
      return;
    }
  }
  //if we didn't return from the loop, we didn't find an update for the joint
  for(uint i=0; i<3; i++) //check to see if it's different from the current
    if(pids[i]!=state->pids[joint][i]) {
      PIDUpdate update(joint,pids); //it is different, insert a new update
      pidchanges.push_back(update);
      break;
    }
  func_end();
}


MotionManager::MC_ID
MotionManager::skip_ahead(MC_ID mcid) const {
  // this is in case a new motion has been added, but the current
  // process hasn't received its own copy yet, so should skip over them
#ifdef PLATFORM_APERIOS
  while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[_MMaccID]==NULL)
    mcid=cmdlist.next(mcid);
  return mcid;
#else
  return cmdlist.next(mcid);
#endif
}

MotionManager::OutputState::OutputState()
  : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
{}
MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
  : priority(pri),mcid(mc), pid(DefaultPIDs[out])
{
  for(unsigned int i=0; i<NumFrames; i++)
    frames[i]=cmds[i];
}
MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
  : priority(pri),mcid(mc), pid(DefaultPIDs[out])
{
  for(unsigned int i=0; i<NumFrames; i++)
    frames[i]=cmd;
}
MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
  : priority(pri),mcid(mc), pid(DefaultPIDs[out])
{
  frames[frame]=cmd;
}
MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputPID& p)
  : priority(pri),mcid(mc), pid(p)
{}
MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
  : priority(pri),mcid(mc), pid(p)
{
  for(unsigned int i=0; i<NumFrames; i++)
    frames[i]=cmds[i];
}
MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
  : priority(pri),mcid(mc), pid(p)
{
  for(unsigned int i=0; i<NumFrames; i++)
    frames[i]=cmd;
}


/*! @file
 * @brief Implements MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
 * @author ejt (Creator)
 *
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.25 $
 * $State: Rel $
 * $Date: 2003/10/07 01:00:40 $
 */


/*
    for(uint f=0;f<NumFrames;f++)
      for(uint i=0; i<NumOutputs; i++)
        outputs[f][i]=0;
    const uint cyctime=128;
    uint ot=get_time()+3*cyctime;
    for(uint f=0;f<NumFrames;f++) {
      uint t=ot+f*FrameTime;
      outputs[f][TopBrLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      t-=cyctime;
      outputs[f][TopLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      outputs[f][TopRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      t-=cyctime;
      outputs[f][MidLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      outputs[f][MidRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      t-=cyctime;
      outputs[f][BotLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
      outputs[f][BotRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
    }
*/

  /*  for(uint output=TlRedLEDOffset-1; output<LEDOffset+NumLEDs-1; output++) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cout << "Out " << output << ": ";
    for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
      cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
    cout << endl;
    }*/


  /*  cout << get_time() << ' ' << size() << endl;
  for(uint output=TlRedLEDOffset; output<LEDOffset+NumLEDs-1; output++) {
    cmdstatelist_t& curstatelist=cmdstates[output];
    cout << "Out " << output << ": ";
    for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
      cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
    cout << endl;
  }
  */

Tekkotsu v1.5

Generated Fri Oct 10 15:51:59 2003 by Doxygen 1.3.4