00001 #include "FollowHeadBehavior.h"
00002 #include "Events/EventRouter.h"
00003 #include "Shared/debuget.h"
00004 #include "Shared/WorldState.h"
00005 #include "Motion/MMAccessor.h"
00006 #include "Motion/HeadPointerMC.h"
00007 #include "Motion/WalkMC.h"
00008 #include "Motion/PIDMC.h"
00009
00010 FollowHeadBehavior::FollowHeadBehavior() :
00011 BehaviorBase(),
00012 head_release(EventBase::buttonEGID,ChinButOffset,EventBase::activateETID,0),
00013 head_lock(EventBase::buttonEGID,ChinButOffset,EventBase::deactivateETID,0),
00014 clock(EventBase::timerEGID,0,EventBase::statusETID,250),
00015 walker_id(MotionManager::invalid_MC_ID)
00016 {}
00017
00018 FollowHeadBehavior::~FollowHeadBehavior() {
00019 if(isActive())
00020 DoStop();
00021 }
00022
00023 void FollowHeadBehavior::DoStart() {
00024 BehaviorBase::DoStart();
00025
00026 walker_id=motman->addPersistentMotion(SharedObject<WalkMC>());
00027
00028 erouter->addListener(this,head_release);
00029 erouter->addListener(this,head_lock);
00030
00031 processEvent(head_lock);
00032 processEvent(clock);
00033 }
00034
00035 void FollowHeadBehavior::DoStop() {
00036
00037 erouter->removeListener(this);
00038
00039 motman->removeMotion(walker_id);
00040 walker_id=MotionManager::invalid_MC_ID;
00041 BehaviorBase::DoStop();
00042 }
00043
00044 void FollowHeadBehavior::processEvent(const EventBase& e) {
00045 if(e==clock) {
00046
00047 float x=-(state->outputs[HeadOffset+TiltOffset]-outputRanges[HeadOffset+TiltOffset][MinRange])/(outputRanges[HeadOffset+TiltOffset][MaxRange]-outputRanges[HeadOffset+TiltOffset][MinRange])*2+1;
00048 float y=(state->outputs[HeadOffset+RollOffset]-outputRanges[HeadOffset+RollOffset][MinRange])/(outputRanges[HeadOffset+RollOffset][MaxRange]-outputRanges[HeadOffset+RollOffset][MinRange])*2-1;
00049 float a=(state->outputs[HeadOffset+PanOffset]-outputRanges[HeadOffset+PanOffset][MinRange])/(outputRanges[HeadOffset+PanOffset][MaxRange]-outputRanges[HeadOffset+PanOffset][MinRange])*2-1;
00050 MMAccessor<WalkMC> walk(walker_id);
00051 walk.mc()->setTargetVelocity(x*WalkMC::MAX_DX,y*WalkMC::MAX_DY,a*WalkMC::MAX_DA);
00052
00053 } else if(e==head_release) {
00054 cout << "release" << endl;
00055 motman->addPrunableMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0));
00056 erouter->addListener(this,clock);
00057
00058 } else if(e==head_lock) {
00059 cout << "lock" << endl;
00060 motman->addPrunableMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,1));
00061 for(unsigned int i=HeadOffset; i<HeadOffset+NumHeadJoints; i++)
00062 motman->setOutput(NULL,i,state->outputs[i]);
00063 cout << state->outputs[HeadOffset+TiltOffset]/M_PI*180 << ' ' << state->outputs[HeadOffset+PanOffset]/M_PI*180 << ' ' << state->outputs[HeadOffset+RollOffset]/M_PI*180 << endl;
00064 erouter->removeListener(this,clock);
00065
00066 } else {
00067 ASSERT(false,"unprocessed event " << e.getName() << endl);
00068 }
00069 }
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081