BatteryMonitorBehavior.hGo to the documentation of this file.00001
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 "IPC/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
00019
00020
00021 class BatteryMonitorBehavior : public BehaviorBase {
00022 public:
00023 static const unsigned int max_t=10000;
00024 static const unsigned int high_power_p=20;
00025 static const unsigned int no_power_p=14;
00026
00027
00028 BatteryMonitorBehavior() : BehaviorBase("BatteryMonitorBehavior"), pose(NULL), pose_id(MotionManager::invalid_MC_ID), led_id(MotionManager::invalid_MC_ID) {}
00029
00030 virtual ~BatteryMonitorBehavior() {}
00031
00032
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
00040 if(shouldWarn())
00041 processEvent(EventBase(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID));
00042 }
00043
00044 virtual void DoStop() {
00045 if(pose!=NULL)
00046 stopWarning();
00047 erouter->removeListener(this);
00048 BehaviorBase::DoStop();
00049 }
00050
00051 virtual void processEvent(const EventBase &event) {
00052 if(event.getGeneratorID()==EventBase::powerEGID) {
00053
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: {
00063 ASSERTRET(pose!=NULL,"Extra timer 1");
00064 setFlipper(true);
00065 unsigned int flipdelay=calcFlipDelay();
00066
00067
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: {
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 static std::string getClassDescription() { return "Reports the current battery status, and starts flicks the ears to warn when it gets too low"; }
00098 virtual std::string getDescription() const { return getClassDescription(); }
00099
00100
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
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
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
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
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;
00146 MotionManager::MC_ID pose_id;
00147 MotionManager::MC_ID led_id;
00148
00149 private:
00150 BatteryMonitorBehavior(const BatteryMonitorBehavior&);
00151 BatteryMonitorBehavior operator=(const BatteryMonitorBehavior&);
00152 };
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165 #endif
|