Homepage
Demos
Overview
Downloads
Dev. Resources
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   // Very simple visual servoing control -- move the head in the direction of the target
00041   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7;
00042   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
00043   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00044 #ifdef TGT_ERS7
00045   //on an ers-7, we want to set the nod joint to look up, since tilt can only look down
00046   headpointer->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MaxRange]);
00047 #else
00048   //on other models (we'll just assume ers-2xx), center the roll joint
00049   headpointer->setJoints(tilt,pan,0);
00050 #endif
00051   motman->checkinMotion(headpointer_id);
00052 }
00053       
00054 /*! @file
00055  * @brief Implements StareAtBallBehavior, which points the head at the ball
00056  * @author tss (Creator)
00057  */
00058 
00059 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4