Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

WalkToTargetMachine.cc

Go to the documentation of this file.
00001 #include "WalkToTargetMachine.h"
00002 #include "Motion/HeadPointerMC.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Vision/Vision.h"
00005 #include "Shared/WorldState.h"
00006 
00007 //! Converts degrees to radians
00008 inline double DtoR(double deg) { return (deg/180.0*M_PI); }
00009 
00010 void WalkToTargetMachine::setup() {
00011   if(close!=NULL)
00012     addTransition(new VisualTargetCloseTrans(this,close,tracking));
00013   if(lost!=NULL)
00014     addTransition(timeout=new TimeOutTrans(this,lost,500));
00015 }
00016 
00017 
00018 void WalkToTargetMachine::DoStart() {
00019   StateNode::DoStart();
00020   headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00021   walker_id = motman->addMotion(SharedObject<WalkMC>());
00022   erouter->addListener(this,EventBase::visionEGID,tracking);
00023   vision->enableEvents(tracking);
00024 }
00025 
00026 void WalkToTargetMachine::DoStop() {
00027   vision->disableEvents(tracking);
00028   erouter->forgetListener(this);
00029   motman->removeMotion(headpointer_id);
00030   motman->removeMotion(walker_id);
00031   StateNode::DoStop();
00032 }
00033 
00034 //this could be cleaned up event-wise (only use a timer when out of view)
00035 void WalkToTargetMachine::processEvent(const EventBase& event) {
00036   if(timeout)
00037     timeout->resetTimer();
00038   static float horiz=0,vert=0;
00039   const VisionEvent *ve = dynamic_cast<const VisionEvent*>(&event);
00040   if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00041     horiz=ve->getCenterX();
00042     vert=ve->getCenterY();
00043   } else
00044     return;
00045 
00046   //    cout << inview << ' ' << horiz << ' ' << vert << endl;
00047 
00048   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00049   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00050   if(tilt<DtoR(-20))
00051     tilt=DtoR(-20);
00052   if(tilt>DtoR(70))
00053     tilt=DtoR(70);
00054   if(pan>DtoR(80))
00055     pan=DtoR(80);
00056   if(pan<DtoR(-80))
00057     pan=DtoR(-80);
00058   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00059   headpointer->setJoints(tilt,pan,0);
00060   motman->checkinMotion(headpointer_id);
00061 
00062   WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
00063   if(pan<-.05 || pan>.05)
00064     walker->setTargetVelocity(100,0,pan);
00065   else
00066     walker->setTargetVelocity(160,0,0);
00067   motman->checkinMotion(walker_id);
00068 }
00069 
00070 /*! @file
00071  * @brief Implements WalkToTargetMachine, a state machine for walking towards a visual target
00072  * @author ejt (Creator)
00073  *
00074  * $Author: ejt $
00075  * $Name: tekkotsu-1_4_1 $
00076  * $Revision: 1.2 $
00077  * $State: Exp $
00078  * $Date: 2003/03/09 02:45:21 $
00079  */
00080 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2