00001
00002 #ifndef INCLUDED_ExploreMachine_h_
00003 #define INCLUDED_ExploreMachine_h_
00004
00005 #include "Behaviors/StateNode.h"
00006 #include "Behaviors/Nodes/WalkNode.h"
00007 #include "Behaviors/Transitions/SmoothCompareTrans.h"
00008 #include <stdlib.h>
00009 #include "Shared/ERS210Info.h"
00010 #include "Shared/ERS220Info.h"
00011 #include "Shared/ERS7Info.h"
00012 #include "Wireless/Socket.h"
00013
00014
00015 class ExploreMachine : public StateNode {
00016 public:
00017
00018 ExploreMachine()
00019 : StateNode("ExploreMachine"), start(NULL), turn(NULL), walkid(MotionManager::invalid_MC_ID)
00020 {
00021 setRetain(false);
00022 }
00023
00024
00025 ExploreMachine(const std::string& nm, StateNode* p=NULL)
00026 : StateNode(nm,p), start(NULL), turn(NULL), walkid(MotionManager::invalid_MC_ID)
00027 {
00028 setRetain(false);
00029 }
00030
00031 virtual void setup() {
00032
00033 unsigned int IRDistOffset;
00034 if(state->robotDesign&WorldState::ERS210Mask)
00035 IRDistOffset=ERS210Info::IRDistOffset;
00036 else if(state->robotDesign&WorldState::ERS220Mask)
00037 IRDistOffset=ERS220Info::IRDistOffset;
00038 else if(state->robotDesign&WorldState::ERS7Mask)
00039 IRDistOffset=ERS7Info::NearIRDistOffset;
00040 else {
00041 serr->printf("ExploreMachine: Unsupported model!\n");
00042 return;
00043 }
00044
00045 SharedObject<WalkMC> walk;
00046 walkid=motman->addMotion(walk);
00047 WalkNode * move=NULL;
00048 start=addNode(turn=new WalkNode(0,0,0.5f,this));
00049 turn->setName(getName()+"::turn");
00050 addNode(move=new WalkNode(150,0,0,this));
00051 move->setName(getName()+"::move");
00052 turn->addTransition(new TimeOutTrans(move,2000));
00053 move->addTransition(new SmoothCompareTrans<float>(turn,&state->sensors[IRDistOffset],CompareTrans<float>::LT,350,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00054 turn->setWalkID(walkid);
00055 move->setWalkID(walkid);
00056 StateNode::setup();
00057
00058
00059 }
00060
00061 virtual void DoStart() {
00062 StateNode::DoStart();
00063 start->DoStart();
00064
00065 erouter->addListener(this,EventBase::stateMachineEGID,(unsigned int)turn);
00066 }
00067
00068 virtual void DoStop() {
00069 erouter->forgetListener(this);
00070 StateNode::DoStop();
00071 }
00072
00073 virtual void teardown() {
00074
00075 motman->removeMotion(walkid);
00076 StateNode::teardown();
00077
00078 }
00079
00080 virtual void processEvent(const EventBase& ) {
00081
00082 float vel=rand()/(float)RAND_MAX*2.0f-1;
00083 if(vel<0)
00084 vel-=.25;
00085 if(vel>0)
00086 vel+=.25;
00087 turn->setAVelocity(vel);
00088 }
00089
00090 protected:
00091 StateNode * start;
00092 WalkNode * turn;
00093 MotionManager::MC_ID walkid;
00094
00095 private:
00096 ExploreMachine(const ExploreMachine&);
00097 ExploreMachine operator=(const ExploreMachine&);
00098 };
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 #endif