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