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",NULL), 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, StateNode* p=NULL)
00031 : StateNode(n,p), 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",this,bandit));
00046 StateNode *left=addNode(new PressNode("Left",this,LFrLegOffset+KneeOffset));
00047 StateNode *right=addNode(new PressNode("Right",this,RFrLegOffset+KneeOffset));
00048 StateNode *decide=addNode(new DecideNode("Decide",this,bandit,left,right));
00049 StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",this,std::cout,wait));
00050 StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",this,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
00087 PressNode(const char* n, StateNode* p, unsigned int idx) : StateNode("PressNode",n,p), press_id(MotionManager::invalid_MC_ID), index(idx) {
00088 SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > press;
00089 press->setPlayTime(0);
00090 press->setOutputCmd(idx,.6);
00091 press->setPlayTime(1);
00092 press->setOutputCmd(idx,.6);
00093 press->setPlayTime(200);
00094 press->setOutputCmd(idx,.3);
00095 press->setPlayTime(1500);
00096 press->setOutputCmd(idx,outputRanges[idx][MinRange]);
00097 press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1);
00098 }
00099
00100 virtual ~PressNode() {
00101 motman->removeMotion(press_id);
00102 }
00103 virtual void DoStart() {
00104 StateNode::DoStart();
00105 MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00106 press->play();
00107 press->setOutputCmd(index,.6);
00108
00109 }
00110 virtual void DoStop() {
00111 MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00112
00113 press->pause();
00114 press->setPlayTime(0);
00115 StateNode::DoStop();
00116 }
00117 protected:
00118 MotionManager::MC_ID press_id;
00119 unsigned int index;
00120 };
00121
00122
00123 class DecideNode : public StateNode {
00124 public:
00125
00126
00127
00128
00129
00130
00131
00132 DecideNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00133 : StateNode("DecideNode",n,p), b(bandito), l(left), r(right)
00134 {}
00135 virtual void DoStart() {
00136 StateNode::DoStart();
00137 AddReference();
00138 DoStop();
00139 if(b.decide()==0) {
00140 std::cout << "Left... " << std::flush;
00141 l->DoStart();
00142 } else {
00143 std::cout << "Right... " << std::flush;
00144 r->DoStart();
00145 }
00146 RemoveReference();
00147 }
00148 protected:
00149 karmedbanditExp3_1& b;
00150 StateNode* l;
00151 StateNode* r;
00152 private:
00153 DecideNode(const DecideNode& node);
00154 DecideNode operator=(const DecideNode& node);
00155 };
00156
00157
00158 class WaitNode : public StateNode {
00159 public:
00160
00161
00162
00163
00164
00165 WaitNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito)
00166 : StateNode("WaitNode",n,p), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00167 {
00168 leds_id=motman->addPersistentMotion(SharedObject<LedMC>());
00169 }
00170
00171 virtual ~WaitNode() {
00172 motman->removeMotion(leds_id);
00173 }
00174 virtual void DoStart() {
00175 StateNode::DoStart();
00176 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00177 erouter->addTimer(this,0,1000,false);
00178 MMAccessor<LedMC> leds(leds_id);
00179 leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00180 }
00181 virtual void DoStop() {
00182 erouter->removeListener(this);
00183 b.reward(reward);
00184 cout << endl;
00185 reward=false;
00186 StateNode::DoStop();
00187 }
00188 virtual void processEvent(const EventBase& event) {
00189 if(event.getGeneratorID()==EventBase::timerEGID) {
00190 sndman->PlayFile("whimper.wav");
00191 } else {
00192 sndman->PlayFile("yipper.wav");
00193 reward=true;
00194 MMAccessor<LedMC> leds(leds_id);
00195 leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00196 }
00197 erouter->removeListener(this);
00198 }
00199 protected:
00200 karmedbanditExp3_1& b;
00201 bool reward;
00202 MotionManager::MC_ID leds_id;
00203 };
00204
00205 StareAtBallBehavior* stare;
00206 StateNode* start;
00207 MotionManager::MC_ID liedown;
00208 karmedbanditExp3_1 bandit;
00209
00210 private:
00211 BanditMachine(const BanditMachine& node);
00212 BanditMachine operator=(const BanditMachine& node);
00213 };
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226 #endif