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 #include "Shared/ERS210Info.h"
00012 #include "Shared/ERS220Info.h"
00013 #include "Shared/ERS7Info.h"
00014
00015
00016 class HeadLevelBehavior : public BehaviorBase {
00017 public:
00018
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
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(HeadPointerMC::GravityRelative,true);
00047 head->noMaxSpeed();
00048 head_id=motman->addMotion(head);
00049 erouter->addListener(this,head_lock);
00050 erouter->addListener(this,head_release);
00051 }
00052
00053 virtual void DoStop() {
00054 erouter->forgetListener(this);
00055 motman->removeMotion(head_id);
00056 head->setMode(HeadPointerMC::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],HeadPointerMC::BodyRelative);
00064 motman->removeMotion(pid_id);
00065 pid_id=MotionManager::invalid_MC_ID;
00066 } else if(event==head_release) {
00067 pid_id=motman->addMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0),MotionManager::kHighPriority,false);
00068
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;
00077 EventBase head_lock;
00078 const SharedObject<HeadPointerMC> head;
00079 MotionManager::MC_ID head_id;
00080 MotionManager::MC_ID pid_id;
00081 };
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 #endif