Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ChaseBallBehavior.cc

Go to the documentation of this file.
00001 #include "ChaseBallBehavior.h"
00002 #include "Events/EventRouter.h"
00003 #include "Events/VisionObjectEvent.h"
00004 #include "Shared/WorldState.h"
00005 #include "Motion/HeadPointerMC.h"
00006 #include "Motion/WalkMC.h"
00007 #include "Shared/WMclass.h"
00008 #include "Shared/ProjectInterface.h"
00009 
00010 //! Converts degrees to radians
00011 inline double DtoR(double deg) { return (deg/180.0*M_PI); }
00012 
00013 void ChaseBallBehavior::DoStart() {
00014   BehaviorBase::DoStart();
00015   headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00016   walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00017   erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00018 }
00019 
00020 void ChaseBallBehavior::DoStop() {
00021   erouter->removeListener(this);
00022   motman->removeMotion(headpointer_id);
00023   motman->removeMotion(walker_id);
00024   BehaviorBase::DoStop();
00025 }
00026 
00027 //this could be cleaned up event-wise (only use a timer when out of view)
00028 void ChaseBallBehavior::processEvent(const EventBase& event) {
00029   WMreg(chase_ball_behavior);
00030   WMvari_(float, horiz, 0, chase_ball_behavior);
00031   WMvari_(float, vert, 0, chase_ball_behavior);
00032 
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   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]);
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(40))
00052     tilt=DtoR(40);
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-2_4_1 $
00068  * $Revision: 1.11 $
00069  * $State: Exp $
00070  * $Date: 2004/10/17 01:16:10 $
00071  */
00072 
00073 

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