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