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 "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
00017
00018
00019 class BatteryMonitorBehavior : public BehaviorBase {
00020 public:
00021 static const unsigned int max_t=10000;
00022 static const unsigned int high_power_p=20;
00023 static const unsigned int no_power_p=14;
00024
00025
00026 BatteryMonitorBehavior() : BehaviorBase(), pose(NULL), pose_id(MotionManager::invalid_MC_ID), led_id(MotionManager::invalid_MC_ID) {}
00027
00028 virtual ~BatteryMonitorBehavior() {}
00029
00030
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
00038 if(shouldWarn())
00039 processEvent(EventBase(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID));
00040 }
00041
00042 virtual void DoStop() {
00043 if(pose!=NULL)
00044 stopWarning();
00045 erouter->forgetListener(this);
00046 BehaviorBase::DoStop();
00047 }
00048
00049 virtual void processEvent(const EventBase &event) {
00050 if(event.getGeneratorID()==EventBase::powerEGID) {
00051
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: {
00061 ASSERTRET(pose!=NULL,"Extra timer 1");
00062 setFlipper(true);
00063 unsigned int flipdelay=calcFlipDelay();
00064
00065
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: {
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
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
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
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
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
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;
00141 MotionManager::MC_ID pose_id;
00142 MotionManager::MC_ID led_id;
00143
00144 private:
00145 BatteryMonitorBehavior(const BatteryMonitorBehavior&);
00146 BatteryMonitorBehavior operator=(const BatteryMonitorBehavior&);
00147 };
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160 #endif