00001 #include "StartupBehavior.h"
00002
00003 #include "Behaviors/Controller.h"
00004 #include "Behaviors/Controls/BatteryCheckControl.h"
00005 #include "Behaviors/Controls/ControlBase.h"
00006 #include "Behaviors/Controls/PostureEditor.h"
00007 #include "Behaviors/Controls/HelpControl.h"
00008 #include "Behaviors/Controls/RebootControl.h"
00009 #include "Behaviors/Controls/ShutdownControl.h"
00010
00011 #include "Motion/MotionCommand.h"
00012 #include "Motion/EmergencyStopMC.h"
00013 #include "Motion/PIDMC.h"
00014 #include "Motion/MotionSequenceMC.h"
00015 #include "Motion/MMAccessor.h"
00016
00017 #include "SoundPlay/SoundManager.h"
00018
00019 #include "Shared/ERS210Info.h"
00020 #include "Shared/ERS7Info.h"
00021
00022 #include "Shared/ProjectInterface.h"
00023
00024 StartupBehavior theStartup;
00025 BehaviorBase& ProjectInterface::startupBehavior=theStartup;
00026
00027 StartupBehavior::StartupBehavior()
00028 : BehaviorBase(), spawned(),setup(),
00029 stop_id(MotionManager::invalid_MC_ID),
00030 pid_id(MotionManager::invalid_MC_ID)
00031 {
00032 AddReference();
00033 }
00034
00035 StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}
00036
00037 void StartupBehavior::DoStart() {
00038 BehaviorBase::DoStart();
00039
00040
00041 initVision();
00042
00043
00044
00045 pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+2,false);
00046
00047
00048 erouter->addTimer(this,0,4*FrameTime*NumFrames,true);
00049
00050
00051 const SharedObject<EmergencyStopMC> stop;
00052
00053
00054
00055
00056 stop->setStopped(true,false);
00057 stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
00058
00059
00060 BatteryCheckControl batchk;
00061 batchk.activate(MotionManager::invalid_MC_ID,NULL);
00062
00063
00064 batchk.deactivate();
00065
00066
00067 Controller * controller=new Controller;
00068 controller->DoStart();
00069 controller->setEStopID(stop_id);
00070 controller->setRoot(SetupMenus());
00071 wireless->setReceiver(sout, Controller::console_callback);
00072 spawned.push_back(controller);
00073
00074 sndman->PlayFile("roar.wav");
00075
00076
00077
00078 unsigned int mouthOffset=-1U;
00079 if(state->robotDesign & WorldState::ERS210Mask)
00080 mouthOffset=ERS210Info::MouthOffset;
00081 if(state->robotDesign & WorldState::ERS7Mask)
00082 mouthOffset=ERS7Info::MouthOffset;
00083 if(mouthOffset!=-1U) {
00084 SharedObject<MotionSequenceMC<MotionSequence::SizeTiny> > closeMouth;
00085 closeMouth->setPlayTime(3000);
00086 closeMouth->setOutputCmd(mouthOffset,outputRanges[mouthOffset][MaxRange]);
00087 closeMouth->setPlayTime(3500);
00088 closeMouth->setOutputCmd(mouthOffset,outputRanges[mouthOffset][MaxRange]);
00089 motman->addMotion(closeMouth,MotionManager::kEmergencyPriority+1,true);
00090 erouter->addTimer(this,1,3250,false);
00091 }
00092
00093
00094
00095
00096
00097 }
00098
00099 void StartupBehavior::DoStop() {
00100 for(std::vector<BehaviorBase*>::iterator it=spawned.begin(); it!=spawned.end(); it++) {
00101 cout << "StartupBehavior stopping spawned: " << (*it)->getName() << endl;
00102 (*it)->DoStop();
00103 }
00104 motman->removeMotion(stop_id);
00105 BehaviorBase::DoStop();
00106 }
00107
00108
00109 void StartupBehavior::processEvent(const EventBase& event) {
00110 if(event.getGeneratorID()==EventBase::timerEGID && event.getSourceID()==0) {
00111
00112
00113 static unsigned int start_time=-1U;
00114 const unsigned int tot_time=2000;
00115 if(start_time==-1U) {
00116 start_time=get_time();
00117 } else {
00118 float power=(get_time()-start_time)/(float)tot_time;
00119 if(power>1)
00120 power=1;
00121 MMAccessor<PIDMC>(pid_id)->setAllPowerLevel(power*power);
00122 }
00123 if((get_time()-start_time)>=tot_time) {
00124 erouter->removeTimer(this,0);
00125 motman->removeMotion(pid_id);
00126 pid_id=MotionManager::invalid_MC_ID;
00127 }
00128 }
00129
00130 if(event.getGeneratorID()==EventBase::timerEGID && event.getSourceID()==1) {
00131
00132
00133
00134 unsigned int mouthOffset=-1U;
00135 if(state->robotDesign & WorldState::ERS210Mask)
00136 mouthOffset=ERS210Info::MouthOffset;
00137 if(state->robotDesign & WorldState::ERS7Mask)
00138 mouthOffset=ERS7Info::MouthOffset;
00139 if(mouthOffset!=-1U) {
00140 MMAccessor<EmergencyStopMC> estop(stop_id);
00141 float weight=estop->getOutputCmd(mouthOffset).weight;
00142 estop->setOutputCmd(mouthOffset,OutputCmd(outputRanges[mouthOffset][MaxRange],weight));
00143 }
00144 }
00145 }
00146
00147 ControlBase*
00148 StartupBehavior::SetupMenus() {
00149 std::cout << "CONTROLLER-INIT..." << std::flush;
00150 ControlBase* root=new ControlBase("Root Control");
00151 setup.push(root);
00152
00153
00154 {
00155 SetupModeSwitch();
00156 SetupBackgroundBehaviors();
00157 SetupTekkotsuMon();
00158 SetupStatusReports();
00159 SetupFileAccess();
00160 SetupWalkEdit();
00161 addItem(new PostureEditor());
00162 SetupVision();
00163 addItem(new ControlBase("Shutdown?","Confirms decision to reboot or shut down"));
00164 startSubMenu();
00165 {
00166 addItem(new ShutdownControl());
00167 addItem(new RebootControl());
00168 }
00169 endSubMenu();
00170 addItem(new HelpControl(root,2));
00171 }
00172
00173 if(endSubMenu()!=root)
00174 cout << "\n*** WARNING *** menu setup stack corrupted" << endl;
00175 cout << "DONE" << endl;
00176 return root;
00177 }
00178
00179 void
00180 StartupBehavior::startSubMenu() {
00181 setup.push(setup.top()->getSlots().back());
00182 }
00183
00184 void
00185 StartupBehavior::addItem(ControlBase * control) {
00186 setup.top()->pushSlot(control);
00187 }
00188
00189 ControlBase*
00190 StartupBehavior::endSubMenu() {
00191 ControlBase * tmp=setup.top();
00192 setup.pop();
00193 return tmp;
00194 }