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
00011 inline double DtoR(double deg) { return (deg/180.0*M_PI); }
00012
00013 void ChaseBallBehavior::DoStart() {
00014 BehaviorBase::DoStart();
00015 headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00016 walker_id = motman->addMotion(SharedObject<WalkMC>());
00017 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00018 }
00019
00020 void ChaseBallBehavior::DoStop() {
00021 erouter->forgetListener(this);
00022 motman->removeMotion(headpointer_id);
00023 motman->removeMotion(walker_id);
00024 BehaviorBase::DoStop();
00025 }
00026
00027
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
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
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073