00001 #include "WalkToTargetMachine.h"
00002 #include "Motion/HeadPointerMC.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Vision/Vision.h"
00005 #include "Shared/WorldState.h"
00006
00007
00008 inline double DtoR(double deg) { return (deg/180.0*M_PI); }
00009
00010 void WalkToTargetMachine::setup() {
00011 if(close!=NULL)
00012 addTransition(new VisualTargetCloseTrans(this,close,tracking));
00013 if(lost!=NULL)
00014 addTransition(timeout=new TimeOutTrans(this,lost,500));
00015 }
00016
00017
00018 void WalkToTargetMachine::DoStart() {
00019 StateNode::DoStart();
00020 headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00021 walker_id = motman->addMotion(SharedObject<WalkMC>());
00022 erouter->addListener(this,EventBase::visionEGID,tracking);
00023 vision->enableEvents(tracking);
00024 }
00025
00026 void WalkToTargetMachine::DoStop() {
00027 vision->disableEvents(tracking);
00028 erouter->forgetListener(this);
00029 motman->removeMotion(headpointer_id);
00030 motman->removeMotion(walker_id);
00031 StateNode::DoStop();
00032 }
00033
00034
00035 void WalkToTargetMachine::processEvent(const EventBase& event) {
00036 if(timeout)
00037 timeout->resetTimer();
00038 static float horiz=0,vert=0;
00039 const VisionEvent *ve = dynamic_cast<const VisionEvent*>(&event);
00040 if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00041 horiz=ve->getCenterX();
00042 vert=ve->getCenterY();
00043 } else
00044 return;
00045
00046
00047
00048 double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00049 double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00050 if(tilt<DtoR(-20))
00051 tilt=DtoR(-20);
00052 if(tilt>DtoR(70))
00053 tilt=DtoR(70);
00054 if(pan>DtoR(80))
00055 pan=DtoR(80);
00056 if(pan<DtoR(-80))
00057 pan=DtoR(-80);
00058 HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00059 headpointer->setJoints(tilt,pan,0);
00060 motman->checkinMotion(headpointer_id);
00061
00062 WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
00063 if(pan<-.05 || pan>.05)
00064 walker->setTargetVelocity(100,0,pan);
00065 else
00066 walker->setTargetVelocity(160,0,0);
00067 motman->checkinMotion(walker_id);
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080