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/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; //!< used to initialize the global ::startupBehavior, used by MMCombo
00025 BehaviorBase& ProjectInterface::startupBehavior=theStartup; //!< used by MMCombo as the init behavior
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(); // this is a global, so there's a global reference
00033 }
00034 
00035 StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}
00036 
00037 void StartupBehavior::DoStart() {
00038   BehaviorBase::DoStart();
00039   //Initialize the Vision pipeline (it's probably a good idea to do this
00040   //first in case later stuff wants to reference the vision stages)
00041   initVision();
00042 
00043   //This will "fade" in the PIDs so the joints don't jerk to full
00044   //power, also looks cooler
00045   pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+2,false);
00046   //also, pause before we start fading in, PIDs take effect right
00047   //away, before the emergencystop is picked up
00048   erouter->addTimer(this,0,4*FrameTime*NumFrames,true);
00049 
00050   //This sets up the default emergency stop
00051   const SharedObject<EmergencyStopMC> stop;
00052   //if you want to start off unpaused, either change 'true' to
00053   //'false', or just comment out the next line (estop's efault is off)
00054   //Note that if you don't want to start in estop, you should then
00055   //also uncomment the line at the end of this function
00056   stop->setStopped(true,false); 
00057   stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
00058   
00059   //This displays the current battery conditions on the console
00060   BatteryCheckControl batchk;
00061   batchk.activate(MotionManager::invalid_MC_ID,NULL);
00062   //  const SharedObject<LedMC> led; //! @todo LedMC's don't support autopruning yet, it should for uses like this
00063   //  batchk.activate(motman->addMotion(led,true));
00064   batchk.deactivate();
00065 
00066   //This is what runs the menu system
00067   Controller * controller=new Controller;
00068   controller->DoStart();
00069   controller->setEStopID(stop_id);
00070   controller->setRoot(SetupMenus()); //this triggers the layout of the menus themselves
00071   wireless->setReceiver(sout, Controller::console_callback);
00072   spawned.push_back(controller);
00073   
00074   sndman->PlayFile("roar.wav");
00075 
00076   //This will close the mouth so it doesn't look stupid or get in the
00077   //way of head motions (ERS-210 or ERS-7 only)
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); //take 3 seconds to close the mouth
00086     closeMouth->setOutputCmd(mouthOffset,outputRanges[mouthOffset][MaxRange]);
00087     closeMouth->setPlayTime(3500); //and hold it for another .5 seconds
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   //if you didn't want to start off paused, you should throw an
00094   //un-estop event.  This will make it clear to any background
00095   //behaviors (namely WorldStateVelDaemon) that we're not in estop
00096   //erouter->postEvent(new EventBase(EventBase::estopEGID,MotionManager::invalid_MC_ID,EventBase::deactivateETID,0));
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 /*!Uses a few timer events at the beginning to fade in the PID values, and closes the mouth too*/
00109 void StartupBehavior::processEvent(const EventBase& event) {
00110   if(event.getGeneratorID()==EventBase::timerEGID && event.getSourceID()==0) {
00111     //this will do the work of fading in the PID values.  It helps the joints
00112     //to power up without twitching
00113     static unsigned int start_time=-1U;
00114     const unsigned int tot_time=2000; 
00115     if(start_time==-1U) { //first time
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     //we're done closing the mouth... set the mouth to closed in the estop too
00132     //otherwise it might twitch a little when the MotionSequence expires and the estop takes over
00133     // (little == +/-.1 radians, aka estop's threshold for resetting an "out of place" joint) (details, details)
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   // additional controls will be submenus of root:
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 }

Tekkotsu v2.1
Generated Tue Mar 16 23:19:15 2004 by Doxygen 1.3.5