Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

BatteryMonitorBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_BatteryMonitorBehavior_h_
00003 #define INCLUDED_BatteryMonitorBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Shared/debuget.h"
00007 #include "Shared/WorldState.h"
00008 #include "Events/EventRouter.h"
00009 #include "Shared/SharedObject.h"
00010 #include "Motion/MotionManager.h"
00011 #include "Motion/PostureMC.h"
00012 #include "Motion/LedMC.h"
00013 #include "Shared/ERS210Info.h"
00014 #include "Shared/ERS220Info.h"
00015 
00016 //! A background behavior which will monitor the power level and flip the ears when appropriate on a 210, or blink the headlight if a 220
00017 /*! Think of this as a simple example class.  For exercise, try using a MotionSequenceMC instead
00018  *  of switching the ears back manually using a PostureMC */
00019 class BatteryMonitorBehavior : public BehaviorBase {
00020 public:
00021   static const unsigned int max_t=10000; //!< max time between ear flips when at "high power" mark
00022   static const unsigned int high_power_p=20; //!< percent of 100 which is point at which to begin warning
00023   static const unsigned int no_power_p=14; //!< percent of 100 at which power will fail (approximate!)
00024 
00025   //! constructor
00026   BatteryMonitorBehavior() : BehaviorBase(), pose(NULL), pose_id(MotionManager::invalid_MC_ID), led_id(MotionManager::invalid_MC_ID) {}
00027   //! destructor
00028   virtual ~BatteryMonitorBehavior() {}
00029 
00030   //! Listens for the PowerSourceID::LowPowerWarnSID
00031   virtual void DoStart() {
00032     BehaviorBase::DoStart();
00033     erouter->addListener(this,EventBase::powerEGID,PowerSourceID::LowPowerWarnSID);
00034     erouter->addListener(this,EventBase::powerEGID,PowerSourceID::ExternalPowerSID);
00035     erouter->addListener(this,EventBase::powerEGID,PowerSourceID::BatteryConnectSID);
00036     erouter->addListener(this,EventBase::powerEGID,PowerSourceID::UpdatedSID);
00037     //if the low power warning is *already* on, better forge an event and send it to myself
00038     if(shouldWarn())
00039       processEvent(EventBase(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID));
00040   }
00041   //! Stops listening for events
00042   virtual void DoStop() {
00043     if(pose!=NULL)
00044       stopWarning();
00045     erouter->forgetListener(this);
00046     BehaviorBase::DoStop();
00047   }
00048   //! Adds a BatteryMonitorMC to motman if power goes low
00049   virtual void processEvent(const EventBase &event) {
00050     if(event.getGeneratorID()==EventBase::powerEGID) {
00051       //just check for low power status
00052       bool shouldwarn=shouldWarn();
00053       if(pose!=NULL && !shouldwarn)
00054         stopWarning();
00055       else if(pose==NULL && shouldwarn)
00056         startWarning();
00057     } else {
00058       ASSERTRET(event.getGeneratorID()==EventBase::timerEGID,"Unrequested event "<<event.getName());
00059       switch(event.getSourceID()) {
00060       case 1: { // toggle the ears (signals low battery), show battery level on LEDs
00061         ASSERTRET(pose!=NULL,"Extra timer 1");
00062         setFlipper(true);
00063         unsigned int flipdelay=calcFlipDelay();
00064         // if we're just constantly flipping the ears, a slight change is needed so the battery
00065         // level isn't obscuring the LED settings
00066         if(flipdelay<=NumFrames*FrameTime) {
00067           static bool on=false;
00068           on=!on;
00069           if(on) {
00070             motman->setPriority(led_id,MotionManager::kEmergencyPriority+1);
00071             MMAccessor<LedMC> led(led_id);
00072             led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major);
00073           } else
00074             motman->setPriority(led_id,MotionManager::kIgnoredPriority);
00075           erouter->addTimer(this,1,128+flipdelay,false);
00076         } else {
00077           motman->setPriority(led_id,MotionManager::kEmergencyPriority+1);
00078           MMAccessor<LedMC> led(led_id);
00079           led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major);
00080           erouter->addTimer(this,2,128,false);
00081         }
00082       } break;
00083       case 2: { // release ear until next flap, hide LEDs display
00084         ASSERTRET(pose!=NULL,"Extra timer 1");
00085         setFlipper(false);
00086         motman->setPriority(led_id,MotionManager::kIgnoredPriority);
00087         erouter->addTimer(this,1,calcFlipDelay(),false);
00088       } break;
00089       default:
00090         ASSERTRET(false,"Unrequested timer " << event.getName());
00091         break;
00092       }
00093     }
00094   }
00095   virtual std::string getName() const { return "BatteryMonitorBehavior"; }
00096   static std::string getClassDescription() { return "Reports the current battery status, and starts flicks the ears to warn when it gets too low"; }
00097 
00098   //! returns true if the warning should be active (power remaining less than high_power_p, no external power, but also checks that a power update has been received)
00099   static bool shouldWarn() { return state!=NULL && state->powerFlags[PowerSourceID::BatteryConnectSID] && (state->sensors[PowerRemainOffset]*100<=high_power_p || state->powerFlags[PowerSourceID::LowPowerWarnSID]) && !state->powerFlags[PowerSourceID::ExternalPowerSID]; }
00100 
00101 protected:
00102   //! adds a pose and a timer to get the ears flipping
00103   void startWarning() {
00104     cout << "LOW BATTERY" << endl;
00105     pose_id=motman->addMotion(SharedObject<PostureMC>(),MotionManager::kEmergencyPriority+1);
00106     pose=(PostureMC*)motman->peekMotion(pose_id);
00107     SharedObject<LedMC> led;
00108     led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major);
00109     led_id=motman->addMotion(led,MotionManager::kEmergencyPriority+1);
00110     setFlipper(true);
00111     erouter->addTimer(this,2,128,false);
00112   }
00113   //! removes pose, in case battery magically charges
00114   void stopWarning() {
00115     cout << "BATTERY GOOD" << endl;
00116     motman->removeMotion(pose_id);
00117     motman->removeMotion(led_id);
00118     led_id=pose_id=MotionManager::invalid_MC_ID;
00119     pose=NULL;
00120     erouter->removeTimer(this,1);
00121     erouter->removeTimer(this,2);
00122   }
00123   //! makes the ears flip more rapidly as power declines.  Flips back and forth once every 15 seconds at 15%, down to flipping constantly at 5%.
00124   unsigned int calcFlipDelay() {
00125     const float high_power=high_power_p/100.0;
00126     const float no_power=no_power_p/100.0;
00127     float cur_power=state->sensors[PowerRemainOffset];
00128     if(cur_power<no_power)
00129       return 0;
00130     return (unsigned int)(max_t*(cur_power-no_power)/(high_power-no_power));
00131   }
00132   //!sets the ears on a 210 or the headlight on a 220 - true toggles current, false clears
00133   void setFlipper(bool set) {
00134     if(state->robotDesign & WorldState::ERS210Mask)
00135       for(unsigned int i=ERS210Info::EarOffset; i<ERS210Info::EarOffset+ERS210Info::NumEarJoints; i++)
00136         pose->setOutputCmd(i,set?!state->outputs[i]:OutputCmd());
00137     if(state->robotDesign & WorldState::ERS220Mask)
00138       pose->setOutputCmd(ERS220Info::RetractableHeadLEDOffset,set?(state->outputs[ERS220Info::RetractableHeadLEDOffset]>.5?0:1):OutputCmd());
00139   }
00140   PostureMC* pose; //!< if we are currently warning of low battery, holds a pose, NULL otherwise
00141   MotionManager::MC_ID pose_id; //!< id of pose if we are currently warning, MotionManager::invalid_MC_ID otherwise
00142   MotionManager::MC_ID led_id; //!< id of LedMC if we are currently warning, MotionManager::invalid_MC_ID otherwise
00143 
00144 private:
00145   BatteryMonitorBehavior(const BatteryMonitorBehavior&); //!< don't copy behaviors
00146   BatteryMonitorBehavior operator=(const BatteryMonitorBehavior&); //!< don't assign behaviors
00147 };
00148 
00149 /*! @file
00150  * @brief Defines BatteryMonitorBehavior, a background behavior to trigger BatteryMonitorMC to warn when the power is low
00151  * @author ejt (Creator)
00152  *
00153  * $Author: ejt $
00154  * $Name: tekkotsu-1_4_1 $
00155  * $Revision: 1.10 $
00156  * $State: Exp $
00157  * $Date: 2003/06/12 18:06:10 $
00158  */
00159 
00160 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2