Homepage Demos Overview Downloads Tutorials Reference
Credits

StareAtBallBehavior.cc

Go to the documentation of this file.
00001 #include "StareAtBallBehavior.h"
00002 #include "Events/EventRouter.h"
00003 #include "Events/VisionObjectEvent.h"
00004 #include "Shared/WorldState.h"
00005 #include "Motion/HeadPointerMC.h"
00006 #include "Shared/ProjectInterface.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->addPersistentMotion(SharedObject<HeadPointerMC>());
00014   erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00015 }
00016 
00017 void StareAtBallBehavior::DoStop() {
00018   erouter->removeListener(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()==ProjectInterface::visBlueBallSID) {
00026     cout << "BLUE" << endl;
00027     } else if(event.getSourceID()==ProjectInterface::visPinkBallSID) {
00028     cout << "PINK" << endl;
00029     }
00030   */
00031 
00032   static float horiz=0,vert=0;
00033   if(event.getGeneratorID()==EventBase::visObjEGID && event.getTypeID()==EventBase::statusETID) {
00034     horiz=static_cast<const VisionObjectEvent*>(&event)->getCenterX();
00035     vert=static_cast<const VisionObjectEvent*>(&event)->getCenterY();
00036   }
00037 
00038   //cout << horiz << ' ' << vert << endl;
00039 
00040   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7;
00041   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
00042   if(tilt<DtoR(-70))
00043     tilt=DtoR(-70);
00044   if(tilt>DtoR(40))
00045     tilt=DtoR(40);
00046   if(pan>DtoR(80))
00047     pan=DtoR(80);
00048   if(pan<DtoR(-80))
00049     pan=DtoR(-80);
00050   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00051   headpointer->setJoints(tilt,pan,0);
00052   motman->checkinMotion(headpointer_id);
00053 }
00054       
00055 /*! @file
00056  * @brief Implements StareAtBallBehavior, which points the head at the ball
00057  * @author tss (Creator)
00058  */
00059 
00060 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:15 2005 by Doxygen 1.4.0