StareAtBallBehavior.cc Source File

00001 #include "StareAtBallBehavior.h"
00002 #include "Events/EventRouter.h"
00003 #include "Events/VisionEvent.h"
00004 #include "Shared/WorldState.h"
00005 #include "Motion/HeadPointerMC.h"
00006 #include "Vision/Vision.h"
00007 
00008 //! Converts degrees to radians
00009 inline double DtoR(double deg) { return (deg/180.0*M_PI); }
00010 
00011 void StareAtBallBehavior::DoStart() {
00012   BehaviorBase::DoStart();
00013   headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00014   erouter->addListener(this,EventBase::visionEGID);
00015 }
00016 
00017 void StareAtBallBehavior::DoStop() {
00018   erouter->forgetListener(this);
00019   motman->removeMotion(headpointer_id);
00020   BehaviorBase::DoStop();
00021 }
00022 
00023 //this could be cleaned up event-wise (only use a timer when out of view)
00024 void StareAtBallBehavior::processEvent(const EventBase& event) {
00025   /*  if(event.getSourceID()==VisionEventNS::RedBallSID) {
00026     cout << "RED" << endl;
00027   } else if(event.getSourceID()==VisionEventNS::PinkBallSID) {
00028     cout << "PINK" << endl;
00029     }*/
00030   
00031 
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   //    cout << inview << ' ' << horiz << ' ' << vert << endl;
00039 
00040   //cout << vert << endl;
00041 
00042   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7;
00043   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
00044   if(tilt<DtoR(-70))
00045     tilt=DtoR(-70);
00046   if(tilt>DtoR(40))
00047     tilt=DtoR(40);
00048   if(pan>DtoR(80))
00049     pan=DtoR(80);
00050   if(pan<DtoR(-80))
00051     pan=DtoR(-80);
00052   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00053   headpointer->setJoints(tilt,pan,0);
00054   motman->checkinMotion(headpointer_id);
00055 }
00056       
00057 /*! @file
00058  * @brief Implements StareAtBallBehavior, which points the head at the ball
00059  * @author tss (Creator)
00060  */
00061 
00062