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

ChaseBallBehavior.cc

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

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