Homepage Demos Overview Downloads Tutorials Reference
Credits

TailWagMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
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 //! A simple motion command for wagging the tail - you can specify period, magnitude, and tilt
00012 class TailWagMC : public MotionCommand {
00013  public:
00014   //!constructor
00015   TailWagMC() : period(500), magnitude(22*M_PI/180), active(true), tilt() { }
00016   //!constructor
00017   TailWagMC(unsigned int cyc_period, float cyc_magnitude) : period(cyc_period), magnitude(cyc_magnitude), active(true), tilt() { }
00018   //!destructor
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); //bug fix thanks L.A.Olsson AT herts ac uk
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); //bug fix thanks L.A.Olsson AT herts ac uk
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; } //!< sets the period of time between swings, in milliseconds
00042   unsigned int getPeriod() { return period; } //!< gets the period of time between swings, in milliseconds
00043   void setMagnitude(double mag) { magnitude=mag; } //!< sets the magnitude of swings, in radians
00044   double getMagnitude() { return magnitude; } //!< gets the magnitude of swings, in radians
00045   void setTilt(double r) { tilt.set(r,1); }  //!< sets the tilt of the tail while wagging, in radians
00046   void unsetTilt() { tilt.unset(); } //!< makes the tilt control unspecified, will let something else control tilt
00047   double getTilt() { return tilt.value; }  //!< sets the tilt of the tail while wagging, in radians
00048   void setActive(bool a) { active=a; } //!< turns the tail wagger on or off
00049   bool getActive() { return active; } //!< returns true if this is currently trying to wag the tail
00050   
00051  protected:
00052   unsigned int period; //!< period of time between swings, in milliseconds
00053   double magnitude; //!< magnitude of swings, in radians
00054   bool active; //!< true if this is currently trying to wag the tail
00055   OutputCmd tilt; //!< holds current setting for the tilt joint
00056   OutputCmd pans[NumFrames]; //!< holds commands for planning ahead the wagging
00057 };
00058 
00059 /*! @file
00060  * @brief Defines TailWagMC, which will wag the tail on a ERS-210 robot.
00061  * @author ejt (Creator)
00062  *
00063  * $Author: ejt $
00064  * $Name: tekkotsu-2_2_2 $
00065  * $Revision: 1.7 $
00066  * $State: Exp $
00067  * $Date: 2004/12/23 01:47:07 $
00068  */
00069 
00070 #endif
00071 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:15 2005 by Doxygen 1.4.0