BanditMachine.hGo to the documentation of this file.00001
00002 #ifndef INCLUDED_BanditMachine_h_
00003 #define INCLUDED_BanditMachine_h_
00004
00005 #include "Behaviors/StateNode.h"
00006 #include "Behaviors/Demos/StareAtBallBehavior.h"
00007 #include "Shared/SharedObject.h"
00008 #include "Motion/PostureMC.h"
00009 #include "Motion/MotionSequenceMC.h"
00010 #include "Motion/LedMC.h"
00011 #include "Behaviors/Transitions/TimeOutTrans.h"
00012 #include "Behaviors/Transitions/SmoothCompareTrans.h"
00013 #include "Behaviors/Nodes/OutputNode.h"
00014 #include "SoundPlay/SoundManager.h"
00015 #include "Shared/ProjectInterface.h"
00016
00017 #include "Behaviors/Demos/karmedbandit.h"
00018
00019
00020 class BanditMachine : public StateNode {
00021 public:
00022
00023 BanditMachine()
00024 : StateNode("BanditMachine","BanditMachine"), stare(NULL), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2)
00025 {
00026 stare=new StareAtBallBehavior();
00027 stare->AddReference();
00028 }
00029
00030 BanditMachine(const char* n)
00031 : StateNode("BanditMachine",n), stare(), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2)
00032 {
00033 stare=new StareAtBallBehavior();
00034 stare->AddReference();
00035 }
00036
00037 virtual ~BanditMachine() {
00038 stare->RemoveReference();
00039 }
00040
00041 static std::string getClassDescription() { return "Plays k-armed bandit with a computer"; }
00042 virtual std::string getDescription() const { return getClassDescription(); }
00043
00044 virtual void setup() {
00045 StateNode *wait=start=addNode(new WaitNode("Wait",bandit));
00046 StateNode *left=addNode(new PressNode("Left",LFrLegOffset+KneeOffset));
00047 StateNode *right=addNode(new PressNode("Right",RFrLegOffset+KneeOffset));
00048 StateNode *decide=addNode(new DecideNode("Decide",bandit,left,right));
00049 StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",std::cout,wait));
00050 StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",std::cout,wait));
00051 left->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00052 right->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00053 wait->addTransition(new TimeOutTrans(decide,2000));
00054 left->addTransition(new TimeOutTrans(recoverl,1500));
00055 right->addTransition(new TimeOutTrans(recoverr,1500));
00056
00057 StateNode::setup();
00058 }
00059
00060 virtual void DoStart() {
00061 StateNode::DoStart();
00062 stare->DoStart();
00063 start->DoStart();
00064 SharedObject<PostureMC> lie("/ms/data/motion/liedown.pos");
00065 lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77);
00066 lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73);
00067 lie->setOutputCmd(LFrLegOffset+KneeOffset,.6);
00068 lie->setOutputCmd(RFrLegOffset+KneeOffset,.6);
00069 liedown=motman->addPrunableMotion(lie);
00070 }
00071
00072 virtual void DoStop() {
00073 motman->removeMotion(liedown);
00074 stare->DoStop();
00075 StateNode::DoStop();
00076 }
00077
00078 protected:
00079
00080 class PressNode : public StateNode {
00081 public:
00082
00083
00084
00085
00086 PressNode(const char* n, unsigned int idx) : StateNode("PressNode",n), press_id(MotionManager::invalid_MC_ID), index(idx) {
00087 SharedObject<SmallMotionSequenceMC> press;
00088 press->setTime(0);
00089 press->setOutputCmd(idx,.6);
00090 press->setTime(1);
00091 press->setOutputCmd(idx,.6);
00092 press->setTime(200);
00093 press->setOutputCmd(idx,.3);
00094 press->setTime(1500);
00095 press->setOutputCmd(idx,outputRanges[idx][MinRange]);
00096 press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1);
00097 }
00098
00099 virtual ~PressNode() {
00100 motman->removeMotion(press_id);
00101 }
00102 virtual void DoStart() {
00103 StateNode::DoStart();
00104 MMAccessor<SmallMotionSequenceMC> press(press_id);
00105 press->play();
00106 press->setOutputCmd(index,.6);
00107
00108 }
00109 virtual void DoStop() {
00110 MMAccessor<SmallMotionSequenceMC> press(press_id);
00111
00112 press->pause();
00113 press->setTime(0);
00114 StateNode::DoStop();
00115 }
00116 protected:
00117 MotionManager::MC_ID press_id;
00118 unsigned int index;
00119 };
00120
00121
00122 class DecideNode : public StateNode {
00123 public:
00124
00125
00126
00127
00128
00129
00130 DecideNode(const char* n, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00131 : StateNode("DecideNode",n), b(bandito), l(left), r(right)
00132 {}
00133 virtual void DoStart() {
00134 StateNode::DoStart();
00135 AddReference();
00136 DoStop();
00137 if(b.decide()==0) {
00138 std::cout << "Left... " << std::flush;
00139 l->DoStart();
00140 } else {
00141 std::cout << "Right... " << std::flush;
00142 r->DoStart();
00143 }
00144 RemoveReference();
00145 }
00146 protected:
00147 karmedbanditExp3_1& b;
00148 StateNode* l;
00149 StateNode* r;
00150 private:
00151 DecideNode(const DecideNode& node);
00152 DecideNode operator=(const DecideNode& node);
00153 };
00154
00155
00156 class WaitNode : public StateNode {
00157 public:
00158
00159
00160
00161
00162 WaitNode(const char* n, karmedbanditExp3_1& bandito)
00163 : StateNode("WaitNode",n), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00164 {
00165 leds_id=motman->addPersistentMotion(SharedObject<LedMC>());
00166 }
00167
00168 virtual ~WaitNode() {
00169 motman->removeMotion(leds_id);
00170 }
00171 virtual void DoStart() {
00172 StateNode::DoStart();
00173 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00174 erouter->addTimer(this,0,1000,false);
00175 MMAccessor<LedMC> leds(leds_id);
00176 leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00177 }
00178 virtual void DoStop() {
00179 erouter->removeListener(this);
00180 b.reward(reward);
00181 cout << endl;
00182 reward=false;
00183 StateNode::DoStop();
00184 }
00185 virtual void processEvent(const EventBase& event) {
00186 if(event.getGeneratorID()==EventBase::timerEGID) {
00187 sndman->PlayFile("whimper.wav");
00188 } else {
00189 sndman->PlayFile("yipper.wav");
00190 reward=true;
00191 MMAccessor<LedMC> leds(leds_id);
00192 leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00193 }
00194 erouter->removeListener(this);
00195 }
00196 protected:
00197 karmedbanditExp3_1& b;
00198 bool reward;
00199 MotionManager::MC_ID leds_id;
00200 };
00201
00202 StareAtBallBehavior* stare;
00203 StateNode* start;
00204 MotionManager::MC_ID liedown;
00205 karmedbanditExp3_1 bandit;
00206
00207 private:
00208 BanditMachine(const BanditMachine& node);
00209 BanditMachine operator=(const BanditMachine& node);
00210 };
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 #endif
|