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

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:40 2004 by Doxygen 1.3.9.1