00001
00002 #ifndef INCLUDED_MotionSequenceMC_h_
00003 #define INCLUDED_MotionSequenceMC_h_
00004
00005 #include "MotionSequenceEngine.h"
00006 #include "MotionCommand.h"
00007 #include "MotionManager.h"
00008 #include "Events/EventBase.h"
00009
00010
00011
00012
00013
00014
00015 template<unsigned int MAXMOVE>
00016 class MotionSequenceMC : public MotionCommand, public MotionSequenceEngine {
00017 public:
00018 static const unsigned int CAPACITY=MAXMOVE;
00019
00020
00021 MotionSequenceMC()
00022 : MotionCommand(), MotionSequenceEngine(), moves(), lastDirty(false)
00023 {
00024 clear();
00025 }
00026
00027 explicit MotionSequenceMC(const std::string& filename)
00028 : MotionCommand(), MotionSequenceEngine(), moves(), lastDirty(false)
00029 {
00030 clear();
00031 loadFile(filename.c_str());
00032 setTime(1);
00033 }
00034
00035 virtual ~MotionSequenceMC() {}
00036
00037 virtual int isDirty() { return isPlaying(); }
00038 virtual int isAlive() { return (playspeed>0) ? (playtime<=endtime) : (playtime>0); }
00039
00040
00041 virtual int updateOutputs() {
00042 MotionSequenceEngine::updateOutputs();
00043 if(!isPlaying()) {
00044 if(lastDirty)
00045 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00046 lastDirty=false;
00047 for(unsigned int i=0; i<NumOutputs; i++)
00048 motman->setOutput(this,i,getOutputCmd(i));
00049 } else {
00050 lastDirty=true;
00051
00052 for(unsigned int i=0; i<NumOutputs; i++) {
00053 Move_idx_t prev=prevs[i],next=nexts[i];
00054 OutputCmd frames[NumFrames];
00055 frames[0]=getOutputCmd(i);
00056 for(unsigned int t=playtime+FrameTime,j=1;j<NumFrames;j++,t+=FrameTime) {
00057 setRange(t,prev,next);
00058 if(next!=invalid_move)
00059 calcOutput(frames[j],t,moves[prev],moves[next]);
00060 else if(hold)
00061 frames[j]=moves[prev].cmd;
00062 }
00063
00064
00065
00066 motman->setOutput(this,i,frames);
00067 }
00068
00069 }
00070 return NumOutputs;
00071
00072
00073 }
00074
00075 virtual void clear() {
00076 moves.clear();
00077 for(unsigned int i=0; i<NumOutputs; i++) {
00078 prevs[i]=starts[i]=moves.new_back();
00079 moves.back().cmd.unset();
00080 moves.back().next=invalid_move;
00081 moves.back().prev=invalid_move;
00082 moves.back().starttime=0;
00083 nexts[i]=invalid_move;
00084 }
00085 setTime(1);
00086 }
00087
00088 virtual unsigned int getMaxFrames() const { return moves.getMaxCapacity(); }
00089 virtual unsigned int getUsedFrames() const { return moves.size(); }
00090
00091 protected:
00092
00093 typedef ListMemBuf<Move,MAXMOVE,Move_idx_t> list_t;
00094
00095
00096 list_t moves;
00097 bool lastDirty;
00098
00099 virtual Move& getKeyFrame(Move_idx_t x) { return moves[x]; }
00100 virtual const Move& getKeyFrame(Move_idx_t x) const { return moves[x]; }
00101 virtual Move_idx_t newKeyFrame() {
00102 Move_idx_t i=moves.new_front();
00103 if(i==invalid_move)
00104 serr->printf("ERROR: MotionSequenceMC %d has run out of memory\n",getID());
00105 return i;
00106 }
00107
00108 virtual void eraseKeyFrame(Move_idx_t x) { moves.erase(x); }
00109
00110 bool setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const {
00111 bool moved=false;
00112 if(next!=invalid_move && moves[next].starttime<=t) {
00113 moved=true;
00114 do {
00115 prev=next;
00116 next=moves[prev].next;
00117 } while(next!=invalid_move && moves[next].starttime<=t);
00118 } else {
00119 while(moves[prev].prev!=invalid_move && t<moves[prev].starttime) {
00120 next=prev;
00121 prev=moves[next].prev;
00122 moved=true;
00123 }
00124 }
00125 return moved;
00126 }
00127 };
00128
00129 typedef MotionSequenceMC<NumOutputs*2> TinyMotionSequenceMC;
00130 typedef MotionSequenceMC<NumOutputs*3> SmallMotionSequenceMC;
00131 typedef MotionSequenceMC<NumOutputs*6> MediumMotionSequenceMC;
00132 typedef MotionSequenceMC<NumOutputs*11> LargeMotionSequenceMC;
00133 typedef MotionSequenceMC<NumOutputs*26> XLargeMotionSequenceMC;
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 #endif