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

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