Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
00001 #include "ChaseBallBehavior.h" 00002 #include "Events/EventRouter.h" 00003 #include "Events/VisionEvent.h" 00004 #include "Shared/WorldState.h" 00005 #include "Motion/HeadPointerMC.h" 00006 #include "Motion/WalkMC.h" 00007 #include "Vision/Vision.h" 00008 00009 //! Converts degrees to radians 00010 inline double DtoR(double deg) { return (deg/180.0*M_PI); } 00011 00012 void ChaseBallBehavior::DoStart() { 00013 BehaviorBase::DoStart(); 00014 headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>()); 00015 walker_id = motman->addMotion(SharedObject<WalkMC>()); 00016 erouter->addListener(this,EventBase::visionEGID); 00017 vision->enableEvents(VisionEventNS::RedBallSID); 00018 vision->enableEvents(VisionEventNS::PinkBallSID); 00019 } 00020 00021 void ChaseBallBehavior::DoStop() { 00022 vision->disableEvents(VisionEventNS::RedBallSID); 00023 vision->disableEvents(VisionEventNS::PinkBallSID); 00024 erouter->forgetListener(this); 00025 motman->removeMotion(headpointer_id); 00026 motman->removeMotion(walker_id); 00027 BehaviorBase::DoStop(); 00028 } 00029 00030 //this could be cleaned up event-wise (only use a timer when out of view) 00031 void ChaseBallBehavior::processEvent(const EventBase& event) { 00032 static float horiz=0,vert=0; 00033 if(event.getGeneratorID()==EventBase::visionEGID && event.getTypeID()==EventBase::statusETID) { 00034 horiz=static_cast<const VisionEvent*>(&event)->getCenterX(); 00035 vert=static_cast<const VisionEvent*>(&event)->getCenterY(); 00036 } 00037 00038 WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id); 00039 if(state->outputs[HeadOffset+PanOffset]<-.05 || state->outputs[HeadOffset+PanOffset]>.05) 00040 walker->setTargetVelocity(100,0,state->outputs[HeadOffset+PanOffset]*2); 00041 else 00042 walker->setTargetVelocity(160,0,0); 00043 motman->checkinMotion(walker_id); 00044 00045 // cout << inview << ' ' << horiz << ' ' << vert << endl; 00046 00047 double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7.5; 00048 double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; 00049 if(tilt<DtoR(-20)) 00050 tilt=DtoR(-20); 00051 if(tilt>DtoR(70)) 00052 tilt=DtoR(70); 00053 if(pan>DtoR(80)) 00054 pan=DtoR(80); 00055 if(pan<DtoR(-80)) 00056 pan=DtoR(-80); 00057 HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id); 00058 headpointer->setJoints(tilt,pan,0); 00059 motman->checkinMotion(headpointer_id); 00060 } 00061 00062 /*! @file 00063 * @brief Implements ChaseBallBehavior, which runs around after whatever the dog sees 00064 * @author ejt (Creator) 00065 * 00066 * $Author: ejt $ 00067 * $Name: tekkotsu-1_4_1 $ 00068 * $Revision: 1.2 $ 00069 * $State: Exp $ 00070 * $Date: 2003/03/09 02:45:21 $ 00071 */ 00072 00073
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2 |