Homepage Demos Overview Downloads Tutorials Reference
Credits

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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:13 2004 by Doxygen 1.3.9.1