Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

StartupBehavior.cc Go to the documentation of this file. #include "StartupBehavior.h" #include "Behaviors/Controller.h" #include "Behaviors/Controls/BatteryCheckControl.h" #include "Behaviors/Controls/ControlBase.h" #include "Behaviors/Controls/PostureEditor.h" #include "Behaviors/Controls/HelpControl.h" #include "Behaviors/Controls/RebootControl.h" #include "Behaviors/Controls/ShutdownControl.h" #include "Motion/MotionCommand.h" #include "Motion/EmergencyStopMC.h" #include "Motion/PIDMC.h" #include "Motion/MotionSequenceMC.h" #include "Motion/MMAccessor.h" #include "SoundPlay/SoundManager.h" #include "Shared/ERS210Info.h" #include "Shared/ERS7Info.h" #include "Shared/ProjectInterface.h" StartupBehavior theStartup; //!< used to initialize the global ::startupBehavior, used by MMCombo BehaviorBase& ProjectInterface::startupBehavior=theStartup; //!< used by MMCombo as the init behavior StartupBehavior::StartupBehavior() : BehaviorBase(), spawned(),setup(), stop_id(MotionManager::invalid_MC_ID), pid_id(MotionManager::invalid_MC_ID) { AddReference(); // this is a global, so there's a global reference } StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;} void StartupBehavior::DoStart() { BehaviorBase::DoStart(); //Initialize the Vision pipeline (it's probably a good idea to do this //first in case later stuff wants to reference the vision stages) initVision(); //This will "fade" in the PIDs so the joints don't jerk to full //power, also looks cooler pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+2,false); //also, pause before we start fading in, PIDs take effect right //away, before the emergencystop is picked up erouter->addTimer(this,0,4*FrameTime*NumFrames,true); //This sets up the default emergency stop const SharedObject<EmergencyStopMC> stop; //if you want to start off unpaused, either change 'true' to //'false', or just comment out the next line (estop's efault is off) //Note that if you don't want to start in estop, you should then //also uncomment the line at the end of this function stop->setStopped(true,false); stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority); //This displays the current battery conditions on the console BatteryCheckControl batchk; batchk.activate(MotionManager::invalid_MC_ID,NULL); // const SharedObject<LedMC> led; //! @todo LedMC's don't support autopruning yet, it should for uses like this // batchk.activate(motman->addMotion(led,true)); batchk.deactivate(); //This is what runs the menu system Controller * controller=new Controller; controller->DoStart(); controller->setEStopID(stop_id); controller->setRoot(SetupMenus()); //this triggers the layout of the menus themselves wireless->setReceiver(sout, Controller::console_callback); spawned.push_back(controller); sndman->PlayFile("roar.wav"); //This will close the mouth so it doesn't look stupid or get in the //way of head motions (ERS-210 or ERS-7 only) unsigned int mouthOffset=-1U; if(state->robotDesign & WorldState::ERS210Mask) mouthOffset=ERS210Info::MouthOffset; if(state->robotDesign & WorldState::ERS7Mask) mouthOffset=ERS7Info::MouthOffset; if(mouthOffset!=-1U) { SharedObject<MotionSequenceMC<MotionSequence::SizeTiny> > closeMouth; closeMouth->setPlayTime(3000); //take 3 seconds to close the mouth closeMouth->setOutputCmd(mouthOffset,outputRanges[mouthOffset][MaxRange]); closeMouth->setPlayTime(3500); //and hold it for another .5 seconds closeMouth->setOutputCmd(mouthOffset,outputRanges[mouthOffset][MaxRange]); motman->addMotion(closeMouth,MotionManager::kEmergencyPriority+1,true); erouter->addTimer(this,1,3250,false); } //if you didn't want to start off paused, you should throw an //un-estop event. This will make it clear to any background //behaviors (namely WorldStateVelDaemon) that we're not in estop //erouter->postEvent(new EventBase(EventBase::estopEGID,MotionManager::invalid_MC_ID,EventBase::deactivateETID,0)); } void StartupBehavior::DoStop() { for(std::vector<BehaviorBase*>::iterator it=spawned.begin(); it!=spawned.end(); it++) (*it)->DoStop(); motman->removeMotion(stop_id); BehaviorBase::DoStop(); } /*!Uses a few timer events at the beginning to fade in the PID values, and closes the mouth too*/ void StartupBehavior::processEvent(const EventBase& event) { if(event.getGeneratorID()==EventBase::timerEGID && event.getSourceID()==0) { //this will do the work of fading in the PID values. It helps the joints //to power up without twitching static unsigned int start_time=-1U; const unsigned int tot_time=2000; if(start_time==-1U) { //first time start_time=get_time(); } else { float power=(get_time()-start_time)/(float)tot_time; if(power>1) power=1; MMAccessor<PIDMC>(pid_id)->setAllPowerLevel(power*power); } if((get_time()-start_time)>=tot_time) { erouter->removeTimer(this,0); motman->removeMotion(pid_id); pid_id=MotionManager::invalid_MC_ID; } } if(event.getGeneratorID()==EventBase::timerEGID && event.getSourceID()==1) { //we're done closing the mouth... set the mouth to closed in the estop too //otherwise it might twitch a little when the MotionSequence expires and the estop takes over // (little == +/-.1 radians, aka estop's threshold for resetting an "out of place" joint) (details, details) unsigned int mouthOffset=-1U; if(state->robotDesign & WorldState::ERS210Mask) mouthOffset=ERS210Info::MouthOffset; if(state->robotDesign & WorldState::ERS7Mask) mouthOffset=ERS7Info::MouthOffset; if(mouthOffset!=-1U) { MMAccessor<EmergencyStopMC> estop(stop_id); float weight=estop->getOutputCmd(mouthOffset).weight; estop->setOutputCmd(mouthOffset,OutputCmd(outputRanges[mouthOffset][MaxRange],weight)); } } } ControlBase* StartupBehavior::SetupMenus() { std::cout << "CONTROLLER-INIT..." << std::flush; ControlBase* root=new ControlBase("Root Control"); setup.push(root); // additional controls will be submenus of root: { SetupModeSwitch(); SetupBackgroundBehaviors(); SetupTekkotsuMon(); SetupStatusReports(); SetupFileAccess(); SetupWalkEdit(); addItem(new PostureEditor()); SetupVision(); addItem(new ControlBase("Shutdown?","Confirms decision to reboot or shut down")); startSubMenu(); { addItem(new ShutdownControl()); addItem(new RebootControl()); } endSubMenu(); addItem(new HelpControl(root,2)); } if(endSubMenu()!=root) cout << "\n*** WARNING *** menu setup stack corrupted" << endl; cout << "DONE" << endl; return root; } void StartupBehavior::startSubMenu() { setup.push(setup.top()->getSlots().back()); } void StartupBehavior::addItem(ControlBase * control) { setup.top()->pushSlot(control); } ControlBase* StartupBehavior::endSubMenu() { ControlBase * tmp=setup.top(); setup.pop(); return tmp; }

Tekkotsu v2.0.1+Doc

Generated Mon Feb 9 22:16:50 2004 by Doxygen 1.3.5