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

StareAtBallBehavior.cc

Go to the documentation of this 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   vision->enableEvents(VisionEventNS::RedBallSID);
00016   vision->enableEvents(VisionEventNS::PinkBallSID);
00017 }
00018 
00019 void StareAtBallBehavior::DoStop() {
00020   vision->disableEvents(VisionEventNS::RedBallSID);
00021   vision->disableEvents(VisionEventNS::PinkBallSID);
00022   erouter->forgetListener(this);
00023   motman->removeMotion(headpointer_id);
00024   BehaviorBase::DoStop();
00025 }
00026 
00027 //this could be cleaned up event-wise (only use a timer when out of view)
00028 void StareAtBallBehavior::processEvent(const EventBase& event) {
00029   /*  if(event.getSourceID()==VisionEventNS::RedBallSID) {
00030     cout << "RED" << endl;
00031   } else if(event.getSourceID()==VisionEventNS::PinkBallSID) {
00032     cout << "PINK" << endl;
00033     }*/
00034   
00035 
00036   static float horiz=0,vert=0;
00037   if(event.getGeneratorID()==EventBase::visionEGID && event.getTypeID()==EventBase::statusETID) {
00038     horiz=static_cast<const VisionEvent*>(&event)->getCenterX();
00039     vert=static_cast<const VisionEvent*>(&event)->getCenterY();
00040   }
00041 
00042   //    cout << inview << ' ' << horiz << ' ' << vert << endl;
00043 
00044   //cout << vert << endl;
00045 
00046   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7;
00047   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
00048   if(tilt<DtoR(-70))
00049     tilt=DtoR(-70);
00050   if(tilt>DtoR(40))
00051     tilt=DtoR(40);
00052   if(pan>DtoR(80))
00053     pan=DtoR(80);
00054   if(pan<DtoR(-80))
00055     pan=DtoR(-80);
00056   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00057   headpointer->setJoints(tilt,pan,0);
00058   motman->checkinMotion(headpointer_id);
00059 }
00060       
00061 /*! @file
00062  * @brief Implements StareAtBallBehavior, which points the head at the ball
00063  * @author tss (Creator)
00064  */
00065 
00066 

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