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