Homepage Demos Overview Downloads Tutorials Reference
Credits

HeadLevelBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_HeadLevelBehavior_h_
00003 #define INCLUDED_HeadLevelBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/MotionManager.h"
00007 #include "Motion/MMAccessor.h"
00008 #include "Motion/OldHeadPointerMC.h"
00009 #include "Motion/PIDMC.h"
00010 #include <math.h>
00011 #include "Shared/ERS210Info.h"
00012 #include "Shared/ERS220Info.h"
00013 #include "Shared/ERS7Info.h"
00014 
00015 //! Tests the head leveling code of OldHeadPointerMC
00016 class HeadLevelBehavior : public BehaviorBase {
00017  public:
00018   //! constructor
00019   HeadLevelBehavior()
00020     : BehaviorBase(),
00021       head_release(EventBase::buttonEGID,0,EventBase::activateETID,0),
00022       head_lock(EventBase::buttonEGID,0,EventBase::deactivateETID,0),
00023       head(),
00024       head_id(MotionManager::invalid_MC_ID), pid_id(MotionManager::invalid_MC_ID)
00025   {
00026     if(state->robotDesign & WorldState::ERS7Mask) {
00027       head_release.setSourceID(ERS7Info::HeadButOffset);
00028       head_lock.setSourceID(ERS7Info::HeadButOffset);
00029     } else if(state->robotDesign & WorldState::ERS210Mask) {
00030       head_release.setSourceID(ERS210Info::HeadFrButOffset);
00031       head_lock.setSourceID(ERS210Info::HeadFrButOffset);
00032     } else if(state->robotDesign & WorldState::ERS220Mask) {
00033       head_release.setSourceID(ERS220Info::HeadFrButOffset);
00034       head_lock.setSourceID(ERS220Info::HeadFrButOffset);
00035     }
00036     head.getRegion()->AddReference();
00037   }
00038   //! destructor
00039   virtual ~HeadLevelBehavior() {
00040     head.getRegion()->RemoveReference();
00041   }
00042 
00043   virtual void DoStart() {
00044     BehaviorBase::DoStart();
00045     head->setJoints(state->outputs[HeadOffset+TiltOffset],state->outputs[HeadOffset+PanOffset],state->outputs[HeadOffset+RollOffset]);
00046     head->setMode(OldHeadPointerMC::GravityRelative,true);
00047     head->noMaxSpeed(); // this is probably pretty safe - the whole point is to keep the head still
00048     head_id=motman->addPersistentMotion(head);
00049     erouter->addListener(this,head_lock);
00050     erouter->addListener(this,head_release);
00051   }
00052 
00053   virtual void DoStop() {
00054     erouter->removeListener(this);
00055     motman->removeMotion(head_id);
00056     head->setMode(OldHeadPointerMC::BodyRelative,false);
00057     BehaviorBase::DoStop();
00058   }
00059 
00060   virtual void processEvent(const EventBase &event) {
00061     if(event==head_lock) {
00062       for(unsigned int i=HeadOffset; i<HeadOffset+NumHeadJoints; i++)
00063         head->setJointValueFromMode((TPROffset_t)(i-HeadOffset),state->outputs[i],OldHeadPointerMC::BodyRelative);
00064       motman->removeMotion(pid_id);
00065       pid_id=MotionManager::invalid_MC_ID;
00066     } else if(event==head_release) {
00067       pid_id=motman->addPersistentMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0),MotionManager::kHighPriority);
00068       //motman->addPersistentMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0));
00069     } else
00070       ASSERTRET(false,"received unasked for event "<<event.getName());
00071   }
00072   virtual std::string getName() const { return "HeadLevelBehavior"; }
00073   static std::string getClassDescription() { return "Uses the internal accelerometers to attempt to keep the head level."; }
00074 
00075  protected:
00076   EventBase head_release; //!< event mask for releasing head (chin button down)
00077   EventBase head_lock;    //!< event mask for locking head (chin button up)
00078   const SharedObject<OldHeadPointerMC> head; //!< might as well just hang on to the whole memory region and reuse it, we can peek for most of our stuff
00079   MotionManager::MC_ID head_id; //!< MCID of headpointer
00080   MotionManager::MC_ID pid_id; //!< MCID of pid controller
00081 };
00082 
00083 /*! @file
00084  * @brief Defines HeadLevelBehavior, which a prototypes head leveler
00085  * @author ejt (Creator)
00086  *
00087  * $Author: ejt $
00088  * $Name: tekkotsu-2_2 $
00089  * $Revision: 1.9 $
00090  * $State: Exp $
00091  * $Date: 2004/10/17 01:16:10 $
00092  */
00093 
00094 #endif 

Tekkotsu v2.2
Generated Tue Oct 19 14:19:14 2004 by Doxygen 1.3.9.1