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("Bandit Machine",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
00043 virtual void setup() {
00044 StateNode *wait=start=addNode(new WaitNode("Wait",this,bandit));
00045 StateNode *left=addNode(new PressNode("Left",this,LFrLegOffset+KneeOffset));
00046 StateNode *right=addNode(new PressNode("Right",this,RFrLegOffset+KneeOffset));
00047 StateNode *decide=addNode(new DecideNode("Decide",this,bandit,left,right));
00048 StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",this,std::cout,wait));
00049 StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",this,std::cout,wait));
00050 left->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00051 right->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00052 wait->addTransition(new TimeOutTrans(decide,2000));
00053 left->addTransition(new TimeOutTrans(recoverl,1500));
00054 right->addTransition(new TimeOutTrans(recoverr,1500));
00055
00056 StateNode::setup();
00057 }
00058
00059 virtual void DoStart() {
00060 StateNode::DoStart();
00061 stare->DoStart();
00062 start->DoStart();
00063 SharedObject<PostureMC> lie("/ms/data/motion/liedown.pos");
00064 lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77);
00065 lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73);
00066 lie->setOutputCmd(LFrLegOffset+KneeOffset,.6);
00067 lie->setOutputCmd(RFrLegOffset+KneeOffset,.6);
00068 liedown=motman->addPrunableMotion(lie);
00069 }
00070
00071 virtual void DoStop() {
00072 motman->removeMotion(liedown);
00073 stare->DoStop();
00074 StateNode::DoStop();
00075 }
00076
00077 protected:
00078
00079 class PressNode : public StateNode {
00080 public:
00081
00082
00083
00084
00085
00086 PressNode(const char* n, StateNode* p, unsigned int idx) : StateNode(n,p), press_id(MotionManager::invalid_MC_ID), index(idx) {
00087 SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > press;
00088 press->setPlayTime(0);
00089 press->setOutputCmd(idx,.6);
00090 press->setPlayTime(1);
00091 press->setOutputCmd(idx,.6);
00092 press->setPlayTime(200);
00093 press->setOutputCmd(idx,.3);
00094 press->setPlayTime(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<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00105 press->play();
00106 press->setOutputCmd(index,.6);
00107
00108 }
00109 virtual void DoStop() {
00110 MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00111
00112 press->pause();
00113 press->setPlayTime(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
00131 DecideNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00132 : StateNode(n,p), b(bandito), l(left), r(right)
00133 {}
00134 virtual void DoStart() {
00135 StateNode::DoStart();
00136 AddReference();
00137 DoStop();
00138 if(b.decide()==0) {
00139 std::cout << "Left... " << std::flush;
00140 l->DoStart();
00141 } else {
00142 std::cout << "Right... " << std::flush;
00143 r->DoStart();
00144 }
00145 RemoveReference();
00146 }
00147 protected:
00148 karmedbanditExp3_1& b;
00149 StateNode* l;
00150 StateNode* r;
00151 private:
00152 DecideNode(const DecideNode& node);
00153 DecideNode operator=(const DecideNode& node);
00154 };
00155
00156
00157 class WaitNode : public StateNode {
00158 public:
00159
00160
00161
00162
00163
00164 WaitNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito)
00165 : StateNode(n,p), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00166 {
00167 leds_id=motman->addPersistentMotion(SharedObject<LedMC>());
00168 }
00169
00170 virtual ~WaitNode() {
00171 motman->removeMotion(leds_id);
00172 }
00173 virtual void DoStart() {
00174 StateNode::DoStart();
00175 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00176 erouter->addTimer(this,0,1000,false);
00177 MMAccessor<LedMC> leds(leds_id);
00178 leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00179 }
00180 virtual void DoStop() {
00181 erouter->removeListener(this);
00182 b.reward(reward);
00183 cout << endl;
00184 reward=false;
00185 StateNode::DoStop();
00186 }
00187 virtual void processEvent(const EventBase& event) {
00188 if(event.getGeneratorID()==EventBase::timerEGID) {
00189 sndman->PlayFile("whimper.wav");
00190 } else {
00191 sndman->PlayFile("yipper.wav");
00192 reward=true;
00193 MMAccessor<LedMC> leds(leds_id);
00194 leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00195 }
00196 erouter->removeListener(this);
00197 }
00198 protected:
00199 karmedbanditExp3_1& b;
00200 bool reward;
00201 MotionManager::MC_ID leds_id;
00202 };
00203
00204 StareAtBallBehavior* stare;
00205 StateNode* start;
00206 MotionManager::MC_ID liedown;
00207 karmedbanditExp3_1 bandit;
00208
00209 private:
00210 BanditMachine(const BanditMachine& node);
00211 BanditMachine operator=(const BanditMachine& node);
00212 };
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225 #endif