Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
StareAtBallBehavior.ccGo 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->addMotion(SharedObject<HeadPointerMC>()); 00014 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); 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()==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.1 |
Generated Tue Mar 16 23:19:15 2004 by Doxygen 1.3.5 |