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