00001
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/HeadPointerMC.h"
00009 #include "Motion/PIDMC.h"
00010 #include <math.h>
00011
00012
00013 class HeadLevelBehavior : public BehaviorBase {
00014 public:
00015
00016 HeadLevelBehavior()
00017 : BehaviorBase(),
00018 head_release(EventBase::buttonEGID,HeadFrButOffset,EventBase::activateETID,0),
00019 head_lock(EventBase::buttonEGID,HeadFrButOffset,EventBase::deactivateETID,0),
00020 head(),
00021 head_id(MotionManager::invalid_MC_ID)
00022 {
00023 head.getRegion()->AddReference();
00024 }
00025
00026 virtual ~HeadLevelBehavior() {
00027 head.getRegion()->RemoveReference();
00028 }
00029
00030 virtual void DoStart() {
00031 BehaviorBase::DoStart();
00032 head->setJoints(state->outputs[HeadOffset+TiltOffset],state->outputs[HeadOffset+PanOffset],state->outputs[HeadOffset+RollOffset]);
00033 head->setMode(HeadPointerMC::GravityRelative,true);
00034 head_id=motman->addMotion(head);
00035 erouter->addListener(this,head_lock);
00036 erouter->addListener(this,head_release);
00037 }
00038
00039 virtual void DoStop() {
00040 erouter->forgetListener(this);
00041 motman->removeMotion(head_id);
00042 head->setMode(HeadPointerMC::BodyRelative,false);
00043 BehaviorBase::DoStop();
00044 }
00045
00046 virtual void processEvent(const EventBase &event) {
00047 if(event==head_lock) {
00048 for(unsigned int i=HeadOffset; i<HeadOffset+NumHeadJoints; i++)
00049 head->setJointValueFromMode((TPROffset_t)(i-HeadOffset),state->outputs[i],HeadPointerMC::BodyRelative);
00050 motman->addMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,1));
00051 } else if(event==head_release) {
00052 motman->addMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0));
00053 } else
00054 ASSERTRET(false,"received unasked for event "<<event.getName());
00055 }
00056 virtual std::string getName() const { return "HeadLevelBehavior"; }
00057 static std::string getClassDescription() { return "Uses the internal accelerometers to attempt to keep the head level."; }
00058
00059 protected:
00060 const EventBase head_release;
00061 const EventBase head_lock;
00062 const SharedObject<HeadPointerMC> head;
00063 MotionManager::MC_ID head_id;
00064 };
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 #endif