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