Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

TailWagMC.h Go to the documentation of this file. //-*-c++-*- #ifndef INCLUDED_TailWagMC_h #define INCLUDED_TailWagMC_h #include "Motion/MotionCommand.h" #include "Motion/MotionManager.h" #include "Shared/get_time.h" #include "math.h" #include "Shared/ERS7Info.h" #include "Shared/ERS210Info.h" #include "Shared/WorldState.h" //! A simple motion command for wagging the tail - you can specify period, magnitude, and tilt class TailWagMC : public MotionCommand { public: //!constructor TailWagMC() : MotionCommand(), period(500), magnitude(22*M_PI/180), offset(0), active(true), last_pan(0.0), last_sign(0), tilt() { } //!constructor TailWagMC(unsigned int cyc_period, float cyc_magnitude) : MotionCommand(), period(cyc_period), magnitude(cyc_magnitude), offset(0), active(true), last_pan(0.0), last_sign(0), tilt() { } //!destructor virtual ~TailWagMC() {} virtual int updateOutputs() { if(!active) return 0; // only ERS-210 and ERS-7 have moveable tails unsigned int tail_pan_offset, tail_tilt_offset; if(state->robotDesign & WorldState::ERS210Mask) { tail_pan_offset = ERS210Info::TailOffset+PanOffset; tail_tilt_offset = ERS210Info::TailOffset+TiltOffset; } else if(state->robotDesign & WorldState::ERS7Mask) { tail_pan_offset = ERS7Info::TailOffset+PanOffset; tail_tilt_offset = ERS7Info::TailOffset+TiltOffset; } else return 0; for(unsigned int i=0; i<NumFrames; i++) { float new_pan = sin((2*M_PI*(get_time()+offset+i*FrameTime))/period)*magnitude; //bug fix thanks L.A.Olsson AT herts ac uk pans[i].set(new_pan); if ( (new_pan-last_pan >= 0 && last_sign == -1) || (new_pan-last_pan < 0 && last_sign == +1) ) postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID)); last_sign = new_pan-last_pan >= 0 ? 1 : -1; last_pan = new_pan; } motman->setOutput(this,tail_pan_offset,pans); motman->setOutput(this,tail_tilt_offset,tilt); return tilt.weight>0?2:1; } virtual int isDirty() { return active; } virtual int isAlive() { return true; } //! sets the period of time between swings, in milliseconds /*! a bit complicated in order to avoid jerking around when the period changes */ void setPeriod(unsigned int p) { float prevPos=((get_time()+offset)%period)/(float)period; period=p; float curPos=(get_time()%period)/(float)period; offset=static_cast<unsigned int>((prevPos-curPos)*period); } unsigned int getPeriod() { return period; } //!< gets the period of time between swings, in milliseconds void setMagnitude(double mag) { magnitude=mag; } //!< sets the magnitude of swings, in radians double getMagnitude() { return magnitude; } //!< gets the magnitude of swings, in radians void setTilt(double r) { tilt.set(r,1); } //!< sets the tilt of the tail while wagging, in radians void unsetTilt() { tilt.unset(); } //!< makes the tilt control unspecified, will let something else control tilt double getTilt() { return tilt.value; } //!< sets the tilt of the tail while wagging, in radians bool getActive() { return active; } //!< returns true if this is currently trying to wag the tail //! turns the tail wagger on or off void setActive(bool a) { if ( active != a ) last_sign = 0; active=a; } protected: unsigned int period; //!< period of time between swings, in milliseconds double magnitude; //!< magnitude of swings, in radians unsigned int offset; //!< offset in the period, only used if period is changed to avoid twitching bool active; //!< true if this is currently trying to wag the tail float last_pan; //!< last tail position int last_sign; //!< sign of tail movement direction OutputCmd tilt; //!< holds current setting for the tilt joint OutputCmd pans[NumFrames]; //!< holds commands for planning ahead the wagging }; /*! @file * @brief Defines TailWagMC, which will wag the tail on a ERS-210 robot. * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.11 $ * $State: Exp $ * $Date: 2006/02/15 19:14:13 $ */ #endif

Tekkotsu v3.0

Generated Wed Oct 4 00:03:46 2006 by Doxygen 1.4.7