00001
00002 #ifndef INCLUDED_TailWagMC_h
00003 #define INCLUDED_TailWagMC_h
00004
00005 #include "Motion/MotionCommand.h"
00006 #include "Motion/MotionManager.h"
00007 #include "Shared/get_time.h"
00008 #include "math.h"
00009 #include "Shared/ERS210Info.h"
00010
00011
00012 class TailWagMC : public MotionCommand {
00013 public:
00014
00015 TailWagMC() : period(500), magnitude(22*M_PI/180), active(true), tilt() { }
00016 TailWagMC(unsigned int cyc_period, float cyc_magnitude) : period(cyc_period), magnitude(cyc_magnitude), active(true), tilt() { }
00017
00018 virtual ~TailWagMC() {}
00019 virtual int updateOutputs() {
00020 if(!active)
00021 return 0;
00022 if(state->robotDesign&WorldState::ERS210Mask) {
00023 for(unsigned int i=0; i<NumFrames; i++)
00024 pans[i].set(sin((2*M_PI*(get_time()+i*FrameTime))/period)*magnitude);
00025 motman->setOutput(this,ERS210Info::TailOffset+PanOffset,pans);
00026 motman->setOutput(this,ERS210Info::TailOffset+TiltOffset,tilt);
00027 return tilt.weight>0?2:1;
00028 } else if(state->robotDesign&WorldState::ERS7Mask) {
00029 for(unsigned int i=0; i<NumFrames; i++)
00030 pans[i].set(sin((2*M_PI*(get_time()+i*FrameTime))/period)*magnitude);
00031 motman->setOutput(this,ERS7Info::TailOffset+PanOffset,pans);
00032 motman->setOutput(this,ERS7Info::TailOffset+TiltOffset,tilt);
00033 return tilt.weight>0?2:1;
00034 } else
00035 return 0;
00036 }
00037 virtual int isDirty() { return active; }
00038 virtual int isAlive() { return true; }
00039
00040 void setPeriod(unsigned int p) { period=p; }
00041 unsigned int getPeriod() { return period; }
00042 void setMagnitude(double mag) { magnitude=mag; }
00043 double getMagnitude() { return magnitude; }
00044 void setTilt(double r) { tilt.set(r,1); }
00045 void unsetTilt() { tilt.unset(); }
00046 double getTilt() { return tilt.value; }
00047 void setActive(bool a) { active=a; }
00048 bool getActive() { return active; }
00049
00050 protected:
00051 unsigned int period;
00052 double magnitude;
00053 bool active;
00054 OutputCmd tilt;
00055 OutputCmd pans[NumFrames];
00056 };
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069 #endif
00070