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