Homepage Demos Overview Downloads Tutorials Reference
Credits

StartupBehavior.cc

Go to the documentation of this file.
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; //!< used to initialize the global ::startupBehavior, used by MMCombo
00026 BehaviorBase& ProjectInterface::startupBehavior=theStartup; //!< used by MMCombo as the init behavior
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(); // this is a global, so there's a global reference
00034 }
00035 
00036 StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}
00037 
00038 void StartupBehavior::DoStart() {
00039   BehaviorBase::DoStart();
00040   //Initialize the Vision pipeline (it's probably a good idea to do this
00041   //first in case later stuff wants to reference the vision stages)
00042   initVision();
00043 
00044   //This will "fade" in the PIDs so the joints don't jerk to full power, also looks cooler
00045   pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+1,false);
00046   //also, pause before we start fading in, PIDs take effect right away, before the emergencystop is picked up
00047   erouter->addTimer(this,0,4*FrameTime*NumFrames,true);
00048 
00049   //This is the default emergency stop
00050   const SharedObject<EmergencyStopMC> stop;
00051   stop->LoadFile("/ms/data/motion/liedown.pos"); //This *should* be replaced by the current position, but just in case, better than setting everything to 0's
00052   stop->setStopped(true,false); //if you want to start off paused
00053   //sndman->StopPlay(); //so it doesn't play the halt sound the first time (but the false in previous line does that now)
00054   stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
00055   
00056   //This displays the current battery conditions
00057   BatteryCheckControl batchk;
00058   batchk.activate(MotionManager::invalid_MC_ID,NULL);
00059   //  const SharedObject<LedMC> led; //! @todo LedMC's don't support autopruning yet, it should for uses like this, or could the one in Controller
00060   //  batchk.activate(motman->addMotion(led,true));
00061   batchk.deactivate();
00062 
00063   //This is for the menu system
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   //This will close the mouth so it doesn't look stupid
00074   //Now done by setting the emergency stop directly in processEvent, but left as demo code:
00075   /*  const SharedObject<PostureMC> closemouth;
00076       closemouth->setJointCmd(MouthOffset,outputRanges[MouthOffset][MaxRange],1);
00077       motman->addMotion(closemouth,MotionCommand::kEmergencyPriority+2,true);
00078   */
00079 
00080   //if you didn't want to start off paused, you should throw an un-estop event:
00081   //erouter->postEvent(EventBase(EventBase::estopEGID,MotionManager::invalid_MC_ID,EventBase::deactivateETID,0));
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 /*!Uses a few timer events at the beginning to fade in the PID values, and closes the mouth too*/
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) { //first time
00096     start_time=get_time();
00097     MMAccessor<EmergencyStopMC>(stop_id)->takeSnapshot(); //take new snapshot with hopefully valid data
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   // additional controls will be submenus of root:
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 }

Tekkotsu v2.0
Generated Wed Jan 21 03:20:30 2004 by Doxygen 1.3.4