Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 |