Homepage Demos Overview Downloads Tutorials Reference
Credits

FollowHeadBehavior.cc

Go to the documentation of this file.
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("FollowHeadBehavior"),
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   //set up the shared motions
00026   walker_id=motman->addPersistentMotion(SharedObject<WalkMC>());
00027   //register for events and timers
00028   erouter->addListener(this,head_release);
00029   erouter->addListener(this,head_lock);
00030   //prime the head pointer and walk so they're going the right way
00031   processEvent(head_lock);
00032   processEvent(clock);
00033 }
00034 
00035 void FollowHeadBehavior::DoStop() {
00036   //remove timers and listeners
00037   erouter->removeListener(this);
00038   //remove motion commands, set them to invalid
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     //x,y,a are percentage of maximum speed in that direction
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]); //doing this prevents the head from jerking back when you released it to where it was before you pressed the button
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 /*! @file
00072  * @brief Implements FollowHeadBehavior, walks where the head is pointing
00073  * @author ejt (Creator)
00074  *
00075  * $Author: ejt $
00076  * $Name $
00077  * $Revision: 1.9 $
00078  * $State: Exp $
00079  * $Date: 2004/11/11 01:45:36 $
00080  */
00081 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:38 2004 by Doxygen 1.3.9.1