00001 #ifndef INCLUDED_CameraBehavior_h_
00002 #define INCLUDED_CameraBehavior_h_
00003
00004 #include "Shared/WorldState.h"
00005 #include "Events/EventRouter.h"
00006 #include "Behaviors/BehaviorBase.h"
00007 #include "Motion/MMAccessor.h"
00008 #include "Motion/MotionManager.h"
00009 #include "Motion/HeadPointerMC.h"
00010 #include "Motion/TailWagMC.h"
00011 #include "Motion/LedMC.h"
00012 #include "Shared/SharedObject.h"
00013
00014
00015
00016
00017
00018
00019 class CameraBehavior : public BehaviorBase {
00020 public:
00021
00022
00023 CameraBehavior() :
00024 BehaviorBase(),
00025 camera_click(EventBase::buttonEGID,HeadFrButOffset,
00026 EventBase::deactivateETID,250),
00027 sensor_update(EventBase::sensorEGID,SensorSourceID::UpdatedSID,
00028 EventBase::statusETID,0),
00029 headpointer_id(MotionManager::invalid_MC_ID),
00030 tailwag_id(MotionManager::invalid_MC_ID),
00031 led_id(MotionManager::invalid_MC_ID)
00032 {cout << "Camera created" << endl;}
00033
00034
00035 virtual ~CameraBehavior() { cout << "Camera destructed" << endl; if(isActive()) DoStop(); }
00036
00037
00038 virtual void DoStart() {
00039 BehaviorBase::DoStart();
00040
00041 SharedObject<HeadPointerMC> headpointer;
00042
00043 headpointer_id=motman->addMotion(headpointer);
00044 if(tailwag_id==MotionManager::invalid_MC_ID)
00045 tailwag_id=motman->addMotion(SharedObject<TailWagMC>());
00046 led_id=motman->addMotion(SharedObject<LedMC>());
00047
00048 erouter->addListener(this,camera_click);
00049 erouter->addListener(this,sensor_update);
00050
00051 motman->setPIDOff(TailOffset+TiltOffset);
00052 motman->setPIDOff(TailOffset+PanOffset);
00053
00054 processEvent(sensor_update);
00055 }
00056
00057
00058 virtual void DoStop() {
00059
00060 motman->setPIDDefault(TailOffset+TiltOffset);
00061 motman->setPIDDefault(TailOffset+PanOffset);
00062
00063 MMAccessor<TailWagMC>(tailwag_id).mc()->setActive(true);
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 erouter->forgetListener(this);
00074
00075 motman->removeMotion(headpointer_id);
00076 headpointer_id=MotionManager::invalid_MC_ID;
00077 motman->removeMotion(led_id);
00078 led_id=MotionManager::invalid_MC_ID;
00079
00080
00081 BehaviorBase::DoStop();
00082 }
00083
00084
00085
00086 virtual void processEvent(const EventBase& e) {
00087 static int timer=0;
00088 if (timer==1) {
00089 MMAccessor<LedMC> led(led_id);
00090 led.mc()->clear();
00091 }
00092 if (e==sensor_update) {
00093 if (timer>0) timer--;
00094 double tilt=state->outputs[TailOffset+TiltOffset]/RAD(22);
00095 double pan=state->outputs[TailOffset+PanOffset]/RAD(22);
00096 cout << tilt << " " << pan << endl;
00097
00098 MMAccessor<HeadPointerMC>(headpointer_id).mc()->setJoints(
00099 .5*(outputRanges[HeadOffset+TiltOffset][0]+outputRanges[HeadOffset+TiltOffset][1]+(outputRanges[HeadOffset+TiltOffset][1]-outputRanges[HeadOffset+TiltOffset][0])*tilt),
00100 .5*(outputRanges[HeadOffset+PanOffset][0]+outputRanges[HeadOffset+PanOffset][1]+(outputRanges[HeadOffset+PanOffset][1]-outputRanges[HeadOffset+PanOffset][0])*pan), 0.0);
00101 } else if (e.equalOrLongerThan(camera_click)) {
00102 MMAccessor<LedMC> led(led_id);
00103 led.mc()->cycle(FaceLEDMask,250,1.0);
00104
00105
00106
00107 timer=30;
00108 }
00109 }
00110 virtual std::string getName() const { return "CameraBehavior"; }
00111 virtual static std::string getClassDescription() { return "Point head with the tail and push a head button to save a picture"; }
00112 protected:
00113 inline double RAD(double x) { return x/180*M_PI; }
00114
00115 const EventBase camera_click;
00116 const EventBase sensor_update;
00117 MotionManager::MC_ID headpointer_id;
00118 MotionManager::MC_ID tailwag_id;
00119 MotionManager::MC_ID led_id;
00120 };
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 #endif