Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WorldState.cc

Go to the documentation of this file.
00001 #include "WorldState.h"
00002 #include "Shared/get_time.h"
00003 #include "Events/EventRouter.h"
00004 #include "ERS210Info.h"
00005 #include "ERS220Info.h"
00006 #include "ERS7Info.h"
00007 #include "Shared/Config.h"
00008 
00009 #ifdef PLATFORM_APERIOS
00010 #  include <OPENR/core_macro.h>
00011 #  include <OPENR/ObjcommTypes.h>
00012 #  include <OPENR/OPENR.h>
00013 #  include <OPENR/OPENRAPI.h>
00014 #  include <OPENR/OPENRMessages.h>
00015 #  include <OPENR/OPower.h>
00016 #else
00017 #  include "Motion/PostureEngine.h"
00018 #endif
00019 
00020 using namespace std;
00021 
00022 #define GETD(cpc) (((float)sensor.GetData(cpc)->frame[lastFrame].value) / 1.0E6f) //!< returns value from OPEN-R, converted from micro in int to base in float
00023 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value) //!< returns value from OPEN-R, as bool
00024 #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value) //!< return value from OPEN-R, as int
00025 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal) //!< returns signal from OPEN-R as word
00026 #define GETDUTY(cpc) (((OJointValue*)(void*)&sensor.GetData(cpc)->frame[lastFrame])->pwmDuty/512.0f) //!< returns duty cycle from OPEN-R as float; -1 (full reverse) to 0 (idle) to 1 (full forward)
00027 
00028 const double WorldState::g=9.80665;
00029 const double WorldState::IROORDist = 900.0;
00030 
00031 #ifdef PLATFORM_APERIOS
00032 WorldState ** WorldState::stateLookupMap=NULL;
00033 WorldState * state=NULL;
00034 #else
00035 WorldStateLookup state;
00036 #endif
00037 
00038 WorldState::WorldState()
00039   : alwaysGenerateStatus(false), vel_x(0), vel_y(0), vel_a(0), vel_time(0),
00040     robotStatus(0), batteryStatus(0),
00041     lastSensorUpdateTime(0), frameNumber(1), framesProcessed(0),
00042     curtime(0)
00043 {
00044   for(unsigned int i=0; i< NumOutputs; i++)
00045     outputs[i]=0;
00046   for(unsigned int i=0; i< NumButtons; i++)
00047     buttons[i]=0;
00048   for(unsigned int i=0; i< NumSensors; i++)
00049     sensors[i]=0;
00050   for(unsigned int i=0; i< NumPIDJoints; i++)
00051     for(unsigned int j=0; j<3; j++)
00052       pids[i][j]=DefaultPIDs[i][j];
00053   for(unsigned int i=0; i< NumPIDJoints; i++)
00054     pidduties[i]=0;
00055   memset(powerFlags,0,sizeof(unsigned int)*PowerSrcID::NumPowerSIDs);
00056   memset(button_times,0,sizeof(unsigned int)*NumButtons);
00057 }
00058 
00059 #ifdef PLATFORM_APERIOS
00060 
00061 /*! This will cause events to be posted */
00062 void WorldState::read(OSensorFrameVectorData& sensor, WorldState* lastState, EventRouter* er) {
00063   //always using GetInfo(0) to get "global" information for the vector, other infos contain metadata for individual data fields
00064   unsigned int newFrameNumber=sensor.GetInfo(0)->frameNumber;
00065   if(frameNumber>=newFrameNumber)
00066     return; //sensors have already been filled in
00067   
00068   curtime=get_time();
00069 
00070   std::vector<EventBase> evtBuf;
00071   unsigned int lastFrame=sensor.GetInfo(0)->numFrames-1;
00072 
00073   if(lastState!=NULL && lastState!=this) {
00074     alwaysGenerateStatus=lastState->alwaysGenerateStatus;
00075     //pid and velocity values will be copied at "completion"
00076     robotStatus=lastState->robotStatus;
00077     batteryStatus=lastState->batteryStatus;
00078     for(unsigned int i=0; i<PowerSrcID::NumPowerSIDs; i++)
00079       powerFlags[i]=lastState->powerFlags[i];
00080     
00081     //important part -- copy over button_times before calls to chkEvent
00082     for(unsigned int i=0; i<NumButtons; i++)
00083       button_times[i]=lastState->button_times[i];
00084   }
00085   
00086   if(RobotName == ERS210Info::TargetName) {
00087     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointLFRotator);
00088     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointLFElevator);
00089     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointLFKnee);
00090     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLFRotator);
00091     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLFElevator);
00092     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointLFKnee);
00093   
00094     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointRFRotator);
00095     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointRFElevator);
00096     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointRFKnee);
00097     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRFRotator);
00098     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRFElevator);
00099     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointRFKnee);
00100 
00101     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointLHRotator);
00102     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointLHElevator);
00103     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointLHKnee);
00104     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLHRotator);
00105     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLHElevator);
00106     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointLHKnee);
00107 
00108     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointRHRotator);
00109     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointRHElevator);
00110     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointRHKnee);
00111     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRHRotator);
00112     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRHElevator);
00113     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointRHKnee);
00114 
00115     // Get head tilt,pan,roll joint angles
00116     outputs[HeadOffset+TiltOffset] = GETD(ERS210Info::CPCJointNeckTilt);
00117     outputs[HeadOffset+PanOffset ] = GETD(ERS210Info::CPCJointNeckPan);
00118     outputs[HeadOffset+RollOffset] = GETD(ERS210Info::CPCJointNeckRoll);
00119     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointNeckTilt);
00120     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS210Info::CPCJointNeckPan);
00121     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS210Info::CPCJointNeckRoll);
00122 
00123 #ifdef TGT_ERS210
00124     unsigned tail = ERS210Info::TailOffset;
00125     unsigned mouth = ERS210Info::MouthOffset;
00126 #elif defined(TGT_ERS2xx)
00127     unsigned tail = ERS2xxInfo::TailOffset;
00128     unsigned mouth = ERS2xxInfo::MouthOffset;
00129 #else
00130     unsigned tail = capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TailOffset]);
00131     unsigned mouth = capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::MouthOffset]);
00132 #endif
00133     outputs[tail+TiltOffset] = GETD(ERS210Info::CPCJointTailTilt);
00134     outputs[tail+PanOffset]  = GETD(ERS210Info::CPCJointTailPan);
00135     pidduties[tail+TiltOffset] = GETDUTY(ERS210Info::CPCJointTailTilt);
00136     pidduties[tail+PanOffset]  = GETDUTY(ERS210Info::CPCJointTailPan);
00137     
00138     outputs[mouth] = GETD(ERS210Info::CPCJointMouth);
00139     pidduties[mouth] = GETDUTY(ERS210Info::CPCJointMouth);
00140 
00141     if(lastState!=NULL && lastState!=this) {
00142       // Copy over new buttons
00143       // ERS2xx happens to match ERS210 offsets for buttons, so nothing special needed...
00144       buttons[ERS210Info::LFrPawOffset]=GETB(ERS210Info::CPCSensorLFPaw);
00145       buttons[ERS210Info::RFrPawOffset]=GETB(ERS210Info::CPCSensorRFPaw);
00146       buttons[ERS210Info::LBkPawOffset]=GETB(ERS210Info::CPCSensorLHPaw);
00147       buttons[ERS210Info::RBkPawOffset]=GETB(ERS210Info::CPCSensorRHPaw);
00148       
00149       buttons[ERS210Info::ChinButOffset]=GETB(ERS210Info::CPCSensorChinSwitch);
00150       buttons[ERS210Info::BackButOffset]=GETB(ERS210Info::CPCSensorBackSwitch);
00151       buttons[ERS210Info::HeadFrButOffset]=GETD(ERS210Info::CPCSensorHeadFrontPressure);
00152       buttons[ERS210Info::HeadBkButOffset]=GETD(ERS210Info::CPCSensorHeadBackPressure);
00153       
00154       //! process changes
00155       chkEvent(evtBuf,lastState);
00156     } else {
00157       // Get foot switches
00158       chkEvent(evtBuf,ERS210Info::LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),buttonNames[ERS210Info::LFrPawOffset]);
00159       chkEvent(evtBuf,ERS210Info::RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),buttonNames[ERS210Info::RFrPawOffset]);
00160       chkEvent(evtBuf,ERS210Info::LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),buttonNames[ERS210Info::LBkPawOffset]);
00161       chkEvent(evtBuf,ERS210Info::RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),buttonNames[ERS210Info::RBkPawOffset]);
00162 
00163       // Get buttons
00164       chkEvent(evtBuf,ERS210Info::ChinButOffset,  GETB(ERS210Info::CPCSensorChinSwitch),buttonNames[ERS210Info::ChinButOffset]);
00165       chkEvent(evtBuf,ERS210Info::BackButOffset,  GETB(ERS210Info::CPCSensorBackSwitch),buttonNames[ERS210Info::BackButOffset]);
00166       chkEvent(evtBuf,ERS210Info::HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),buttonNames[ERS210Info::HeadFrButOffset]);
00167       chkEvent(evtBuf,ERS210Info::HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),buttonNames[ERS210Info::HeadBkButOffset]);
00168     }
00169 
00170     // Get IR distance sensor
00171     sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f;
00172 
00173     // Get acceleration sensors
00174     sensors[BAccelOffset] = GETD(ERS210Info::CPCSensorAccelFB);
00175     sensors[LAccelOffset] = GETD(ERS210Info::CPCSensorAccelLR);
00176     sensors[DAccelOffset] = GETD(ERS210Info::CPCSensorAccelUD);
00177 
00178     sensors[ERS210Info::ThermoOffset] = GETD(ERS210Info::CPCSensorThermoSensor);
00179   }
00180 
00181   // (ERS-220 only)
00182   if(RobotName == ERS220Info::TargetName) {
00183     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLFRotator);
00184     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLFElevator);
00185     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLFKnee);
00186     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLFRotator);
00187     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLFElevator);
00188     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLFKnee);
00189   
00190     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRFRotator);
00191     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRFElevator);
00192     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRFKnee);
00193     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRFRotator);
00194     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRFElevator);
00195     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRFKnee);
00196   
00197     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLHRotator);
00198     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLHElevator);
00199     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLHKnee);
00200     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLHRotator);
00201     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLHElevator);
00202     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLHKnee);
00203 
00204     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRHRotator);
00205     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRHElevator);
00206     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRHKnee);
00207     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRHRotator);
00208     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRHElevator);
00209     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRHKnee);
00210 
00211     // Get head tilt,pan,roll joint angles
00212     outputs[HeadOffset+TiltOffset] = GETD(ERS220Info::CPCJointNeckTilt);
00213     outputs[HeadOffset+PanOffset ] = GETD(ERS220Info::CPCJointNeckPan);
00214     outputs[HeadOffset+RollOffset] = GETD(ERS220Info::CPCJointNeckRoll);
00215     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS220Info::CPCJointNeckTilt);
00216     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS220Info::CPCJointNeckPan);
00217     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS220Info::CPCJointNeckRoll);
00218 
00219     if(lastState!=NULL && lastState!=this) {
00220       // Copy over new buttons
00221       // ERS2xx happens to match ERS220 offsets for buttons, so nothing special needed...
00222       buttons[ERS220Info::LFrPawOffset]=GETB(ERS220Info::CPCSensorLFPaw);
00223       buttons[ERS220Info::RFrPawOffset]=GETB(ERS220Info::CPCSensorRFPaw);
00224       buttons[ERS220Info::LBkPawOffset]=GETB(ERS220Info::CPCSensorLHPaw);
00225       buttons[ERS220Info::RBkPawOffset]=GETB(ERS220Info::CPCSensorRHPaw);
00226       
00227       buttons[ERS220Info::ChinButOffset]=GETB(ERS220Info::CPCSensorChinSwitch);
00228       buttons[ERS220Info::BackButOffset]=GETB(ERS220Info::CPCSensorBackSwitch);
00229       buttons[ERS220Info::HeadFrButOffset]=GETD(ERS220Info::CPCSensorHeadFrontPressure);
00230       buttons[ERS220Info::HeadBkButOffset]=GETD(ERS220Info::CPCSensorHeadBackPressure);
00231       buttons[ERS220Info::TailLeftButOffset]=GETB(ERS220Info::CPCSensorTailLeftSwitch);
00232       buttons[ERS220Info::TailCenterButOffset]=GETB(ERS220Info::CPCSensorTailCenterSwitch);
00233       buttons[ERS220Info::TailRightButOffset]=GETB(ERS220Info::CPCSensorTailRightSwitch);
00234       
00235       //! process changes
00236       chkEvent(evtBuf,lastState);
00237     } else {
00238       // Get foot switches
00239       chkEvent(evtBuf,ERS220Info::LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),buttonNames[ERS220Info::LFrPawOffset]);
00240       chkEvent(evtBuf,ERS220Info::RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),buttonNames[ERS220Info::RFrPawOffset]);
00241       chkEvent(evtBuf,ERS220Info::LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),buttonNames[ERS220Info::LBkPawOffset]);
00242       chkEvent(evtBuf,ERS220Info::RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),buttonNames[ERS220Info::RBkPawOffset]);
00243 
00244       // Get buttons
00245       chkEvent(evtBuf,ERS220Info::ChinButOffset,  GETB(ERS220Info::CPCSensorChinSwitch),buttonNames[ERS220Info::ChinButOffset]);
00246       chkEvent(evtBuf,ERS220Info::BackButOffset,  GETB(ERS220Info::CPCSensorBackSwitch),buttonNames[ERS220Info::BackButOffset]);
00247       chkEvent(evtBuf,ERS220Info::HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),buttonNames[ERS220Info::HeadFrButOffset]);
00248       chkEvent(evtBuf,ERS220Info::HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),buttonNames[ERS220Info::HeadBkButOffset]);
00249       chkEvent(evtBuf,ERS220Info::TailLeftButOffset, GETB(ERS220Info::CPCSensorTailLeftSwitch),  buttonNames[ERS220Info::TailLeftButOffset]);
00250       chkEvent(evtBuf,ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),buttonNames[ERS220Info::TailCenterButOffset]);
00251       chkEvent(evtBuf,ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), buttonNames[ERS220Info::TailRightButOffset]);
00252     }
00253 
00254     // Get IR distance sensor
00255     sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f;
00256 
00257     // Get acceleration sensors
00258     sensors[BAccelOffset] = GETD(ERS220Info::CPCSensorAccelFB);
00259     sensors[LAccelOffset] = GETD(ERS220Info::CPCSensorAccelLR);
00260     sensors[DAccelOffset] = GETD(ERS220Info::CPCSensorAccelUD);
00261 
00262     sensors[ERS220Info::ThermoOffset] = GETD(ERS220Info::CPCSensorThermoSensor);
00263   }
00264 
00265   // (ERS-7 only)
00266   if(RobotName == ERS7Info::TargetName) {
00267     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointLFRotator);
00268     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointLFElevator);
00269     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointLFKnee);
00270     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLFRotator);
00271     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLFElevator);
00272     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointLFKnee);
00273   
00274     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointRFRotator);
00275     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointRFElevator);
00276     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointRFKnee);
00277     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRFRotator);
00278     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRFElevator);
00279     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointRFKnee);
00280   
00281     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointLHRotator);
00282     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointLHElevator);
00283     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointLHKnee);
00284     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLHRotator);
00285     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLHElevator);
00286     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointLHKnee);
00287 
00288     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointRHRotator);
00289     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointRHElevator);
00290     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointRHKnee);
00291     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRHRotator);
00292     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRHElevator);
00293     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointRHKnee);
00294 
00295     // Get head tilt,pan,nod joint angles
00296     outputs[HeadOffset+TiltOffset] = GETD(ERS7Info::CPCJointNeckTilt);
00297     outputs[HeadOffset+PanOffset ] = GETD(ERS7Info::CPCJointNeckPan);
00298     outputs[HeadOffset+RollOffset] = GETD(ERS7Info::CPCJointNeckNod);
00299     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointNeckTilt);
00300     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS7Info::CPCJointNeckPan);
00301     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS7Info::CPCJointNeckNod);
00302 
00303     outputs[ERS7Info::TailOffset+TiltOffset] = GETD(ERS7Info::CPCJointTailTilt);
00304     outputs[ERS7Info::TailOffset+PanOffset]  = GETD(ERS7Info::CPCJointTailPan);
00305     pidduties[ERS7Info::TailOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointTailTilt);
00306     pidduties[ERS7Info::TailOffset+PanOffset]  = GETDUTY(ERS7Info::CPCJointTailPan);
00307     
00308     outputs[ERS7Info::MouthOffset] = GETD(ERS7Info::CPCJointMouth);
00309     pidduties[ERS7Info::MouthOffset] = GETDUTY(ERS7Info::CPCJointMouth);
00310 
00311     if(lastState!=NULL && lastState!=this) {
00312       // Copy over new buttons
00313       buttons[ERS7Info::LFrPawOffset]=GETB(ERS7Info::CPCSwitchLFPaw);
00314       buttons[ERS7Info::RFrPawOffset]=GETB(ERS7Info::CPCSwitchRFPaw);
00315       buttons[ERS7Info::LBkPawOffset]=GETB(ERS7Info::CPCSwitchLHPaw);
00316       buttons[ERS7Info::RBkPawOffset]=GETB(ERS7Info::CPCSwitchRHPaw);
00317       
00318       // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210
00319       buttons[ ERS7Info::ChinButOffset]=GETSENSOR(ERS7Info::CPCSwitchChin);
00320       buttons[ ERS7Info::HeadButOffset]=GETSENSOR(ERS7Info::CPCSensorHead)/120;
00321       buttons[ ERS7Info::FrontBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackFront)/150;
00322       buttons[ ERS7Info::MiddleBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150;
00323       buttons[ ERS7Info::RearBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackRear)/150;
00324       buttons[ ERS7Info::WirelessSwOffset]=GETSENSOR(ERS7Info::CPCSwitchWireless);
00325       
00326       //! process changes
00327       chkEvent(evtBuf,lastState);
00328     } else {
00329       // Get foot switches
00330       chkEvent(evtBuf,ERS7Info::LFrPawOffset,GETB(ERS7Info::CPCSwitchLFPaw),buttonNames[ERS7Info::LFrPawOffset]);
00331       chkEvent(evtBuf,ERS7Info::RFrPawOffset,GETB(ERS7Info::CPCSwitchRFPaw),buttonNames[ERS7Info::RFrPawOffset]);
00332       chkEvent(evtBuf,ERS7Info::LBkPawOffset,GETB(ERS7Info::CPCSwitchLHPaw),buttonNames[ERS7Info::LBkPawOffset]);
00333       chkEvent(evtBuf,ERS7Info::RBkPawOffset,GETB(ERS7Info::CPCSwitchRHPaw),buttonNames[ERS7Info::RBkPawOffset]);
00334 
00335       // Get buttons/switches
00336       // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210
00337       chkEvent(evtBuf, ERS7Info::ChinButOffset,       GETSENSOR(ERS7Info::CPCSwitchChin),      buttonNames[ERS7Info::ChinButOffset]);
00338       chkEvent(evtBuf, ERS7Info::HeadButOffset,       GETSENSOR(ERS7Info::CPCSensorHead)/120,      buttonNames[ERS7Info::HeadButOffset]);
00339       chkEvent(evtBuf, ERS7Info::FrontBackButOffset,  GETSENSOR(ERS7Info::CPCSensorBackFront)/150, buttonNames[ERS7Info::FrontBackButOffset]);
00340       chkEvent(evtBuf, ERS7Info::MiddleBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150,buttonNames[ERS7Info::MiddleBackButOffset]);
00341       chkEvent(evtBuf, ERS7Info::RearBackButOffset,   GETSENSOR(ERS7Info::CPCSensorBackRear)/150,  buttonNames[ERS7Info::RearBackButOffset]);
00342       chkEvent(evtBuf, ERS7Info::WirelessSwOffset,GETSENSOR(ERS7Info::CPCSwitchWireless),  buttonNames[ERS7Info::WirelessSwOffset]);
00343     }
00344 
00345     // Get IR distance sensor
00346     sensors[ERS7Info::NearIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorNearPSD) / 1000.0f;
00347     sensors[ERS7Info::FarIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorFarPSD) / 1000.0f;
00348     sensors[ERS7Info::ChestIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorChestPSD) / 1000.0f;
00349 
00350     // Get acceleration sensors
00351     sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB);
00352     sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR);
00353     sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD);
00354   }
00355 
00356   //unsigned int dif=curtime-(lastState==NULL ? lastSensorUpdateTime : lastState->lastSensorUpdateTime);
00357   lastSensorUpdateTime=curtime;
00358   frameNumber=newFrameNumber;
00359   framesProcessed=(lastState!=NULL?lastState->framesProcessed:framesProcessed)+1;
00360   
00361   //Apply sensor calibrations (currently only joint positions - perhaps IR as well?)
00362   /*
00363   for(unsigned int i=0; i<NumPIDJoints; i++)
00364     outputs[PIDJointOffset+i]*=config->motion.calibration[i];
00365   */
00366   applyCalibration();
00367 
00368   for(unsigned int i=0; i<evtBuf.size(); i++)
00369     er->postEvent(evtBuf[i]);
00370 
00371   //this version of read doesn't post the sensor update event -- the caller should do that
00372   //this event gets posted by MMCombo only if there's no back log on the interprocess event queue (don't want to stack these up for sensor frames missed by main)
00373   //er->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1);
00374 }
00375 
00376 /*! This will cause events to be posted */
00377 void WorldState::read(const OPowerStatus& power, EventRouter* er) {
00378   std::string actnames[PowerSrcID::NumPowerSIDs];
00379   std::string denames[PowerSrcID::NumPowerSIDs];
00380   unsigned int actmasks[PowerSrcID::NumPowerSIDs];
00381   memset(actmasks,0,sizeof(unsigned int)*PowerSrcID::NumPowerSIDs);
00382 
00383   //RobotStatus
00384   chkPowerEvent(PowerSrcID::PauseSID,          power.robotStatus,orsbPAUSE,                        "Pause",actnames,denames,actmasks);
00385   chkPowerEvent(PowerSrcID::MotorPowerSID,     power.robotStatus,orsbMOTOR_POWER,                  "MotorPower",actnames,denames,actmasks);
00386   chkPowerEvent(PowerSrcID::VibrationSID,      power.robotStatus,orsbVIBRATION_DETECT,             "Vibration",actnames,denames,actmasks);
00387   chkPowerEvent(PowerSrcID::ExternalPortSID,   power.robotStatus,orsbEX_PORT_CONNECTED,            "ExternalPort",actnames,denames,actmasks);
00388   chkPowerEvent(PowerSrcID::StationConnectSID, power.robotStatus,orsbSTATION_CONNECTED,            "StationConnect",actnames,denames,actmasks);
00389   chkPowerEvent(PowerSrcID::ExternalPowerSID,  power.robotStatus,orsbEX_POWER_CONNECTED,           "ExternalPower",actnames,denames,actmasks);
00390   chkPowerEvent(PowerSrcID::BatteryConnectSID, power.robotStatus,orsbBATTERY_CONNECTED,            "BatteryConnect",actnames,denames,actmasks);
00391   chkPowerEvent(PowerSrcID::ChargingSID,       power.robotStatus,orsbBATTERY_CHARGING,             "BatteryCharging",actnames,denames,actmasks);
00392   chkPowerEvent(PowerSrcID::BatteryFullSID,    power.robotStatus,orsbBATTERY_CAPACITY_FULL,        "BatteryFull",actnames,denames,actmasks);
00393   chkPowerEvent(PowerSrcID::LowPowerWarnSID,   power.robotStatus,orsbBATTERY_CAPACITY_LOW,         "BatteryLow",actnames,denames,actmasks);
00394   chkPowerEvent(PowerSrcID::OverChargedSID,    power.robotStatus,orsbBATTERY_OVER_CURRENT,         "BatteryOverCurrent",actnames,denames,actmasks);
00395   chkPowerEvent(PowerSrcID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_DISCHARGING,"BatteryOverTempDischarge",actnames,denames,actmasks);
00396   chkPowerEvent(PowerSrcID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_CHARGING,   "BatteryOverTempCharge",actnames,denames,actmasks);
00397   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbBATTERY_ERROR_OF_CHARGING,    "BatteryChargeError",actnames,denames,actmasks);
00398   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbERROR_OF_PLUNGER,             "PlungerError",actnames,denames,actmasks);
00399   chkPowerEvent(PowerSrcID::PowerGoodSID,      power.robotStatus,orsbOPEN_R_POWER_GOOD,            "PowerGood",actnames,denames,actmasks);
00400   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbERROR_OF_FAN,                 "FanError",actnames,denames,actmasks);
00401   chkPowerEvent(PowerSrcID::DataFromStationSID,power.robotStatus,orsbDATA_STREAM_FROM_STATION,     "DataFromStation",actnames,denames,actmasks);
00402   chkPowerEvent(PowerSrcID::RegisterUpdateSID, power.robotStatus,orsbREGISTER_UPDATED_BY_STATION,  "RegisterUpdate",actnames,denames,actmasks);
00403   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbRTC_ERROR,                    "RTCError",actnames,denames,actmasks);
00404   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_OVERFLOW,                 "RTCOverflow",actnames,denames,actmasks);
00405   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_RESET,                    "RTCReset",actnames,denames,actmasks);
00406   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_SET,                      "RTCSet",actnames,denames,actmasks);
00407   chkPowerEvent(PowerSrcID::SpecialModeSID,    power.robotStatus,orsbSPECIAL_MODE,                 "SpecialMode",actnames,denames,actmasks);
00408   chkPowerEvent(PowerSrcID::BMNDebugModeSID,   power.robotStatus,orsbBMN_DEBUG_MODE,               "BMNDebugMode",actnames,denames,actmasks);
00409   chkPowerEvent(PowerSrcID::ChargerStatusSID,  power.robotStatus,orsbCHARGER_STATUS,               "ChargerStatus",actnames,denames,actmasks);
00410   chkPowerEvent(PowerSrcID::PlungerSID,        power.robotStatus,orsbPLUNGER,                      "Plunger",actnames,denames,actmasks);
00411   chkPowerEvent(PowerSrcID::SuspendedSID,      power.robotStatus,orsbSUSPENDED,                    "Suspended",actnames,denames,actmasks);
00412 
00413   //BatteryStatus
00414   chkPowerEvent(PowerSrcID::ErrorSID,        power.batteryStatus,obsbERROR_CODE_MASK,             "BatteryError",actnames,denames,actmasks);
00415   chkPowerEvent(PowerSrcID::BatteryEmptySID, power.batteryStatus,obsbFULLY_DISCHARGED,            "FullyDischarged",actnames,denames,actmasks);
00416   chkPowerEvent(PowerSrcID::BatteryFullSID,  power.batteryStatus,obsbFULLY_CHARGED,               "FullyCharged",actnames,denames,actmasks);
00417   chkPowerEvent(PowerSrcID::DischargingSID,  power.batteryStatus,obsbDISCHARGING,                 "Discharging",actnames,denames,actmasks);
00418   chkPowerEvent(PowerSrcID::BatteryInitSID,  power.batteryStatus,obsbINITIALIZED,                 "BatteryInit",actnames,denames,actmasks);
00419   chkPowerEvent(PowerSrcID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_TIME_ALARM,        "RemainingTimeAlarm",actnames,denames,actmasks);
00420   chkPowerEvent(PowerSrcID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_CAPACITY_ALARM,    "RemainingCapacityAlarm",actnames,denames,actmasks);
00421   chkPowerEvent(PowerSrcID::TermDischargeSID,power.batteryStatus,obsbTERMINATED_DISCHARGING_ALARM,"TermDischargeAlarm",actnames,denames,actmasks);
00422   chkPowerEvent(PowerSrcID::OverheatingSID,  power.batteryStatus,obsbOVER_TEMP_ALARM,             "OverTempAlarm",actnames,denames,actmasks);
00423   chkPowerEvent(PowerSrcID::TermChargeSID,   power.batteryStatus,obsbTERMINATED_CHARGING_ALARM,   "TermChargeAlarm",actnames,denames,actmasks);
00424   chkPowerEvent(PowerSrcID::OverChargedSID,  power.batteryStatus,obsbOVER_CHARGED_ALARM,          "OverChargeAlarm",actnames,denames,actmasks);
00425   
00426   sensors[PowerRemainOffset] = power.remainingCapacity/100.0;
00427   sensors[PowerThermoOffset] = power.temperature/100.0;
00428   sensors[PowerCapacityOffset] = power.fullyChargedCapacity;
00429   sensors[PowerVoltageOffset] = power.voltage/1000.0;
00430   sensors[PowerCurrentOffset] = power.current;
00431 
00432   //only generate status events when a change happens
00433   for(unsigned int i=0; i<PowerSrcID::NumPowerSIDs; i++) {
00434     if(actmasks[i]) { //now on
00435       if(!powerFlags[i]) //was off: activation
00436         er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00437       else if(actmasks[i]!=powerFlags[i]) //already on - change? : status
00438         er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00439     } else { // now off
00440       if(powerFlags[i]) //was on: deactivation
00441         er->postEvent(EventBase::powerEGID,i,EventBase::deactivateETID,0,denames[i],0);
00442     }
00443     powerFlags[i]=actmasks[i];
00444   }
00445 
00446   er->postEvent(EventBase::powerEGID,PowerSrcID::UpdatedSID,EventBase::statusETID,0,"PowerSrcID::UpdatedSID",1);
00447 }
00448 
00449 #else  //PLATFORM_LOCAL
00450 
00451 /*! This version of read() doesn't post the sensor update event -- the caller should do that
00452  *  (this variant is used by the simulator, different parts fill in different sections, the last one should trigger the event)
00453  *
00454  *  This function assumes that the pose (and any associated sensor readings)
00455  *  are for the current model robot, and that no calibration has already 
00456  *  been applied (so the values must be calibrated)
00457  */
00458 void WorldState::read(const PostureEngine& pose, WorldState* lastState, EventRouter* er) {
00459   curtime=get_time();
00460   std::vector<EventBase> evtBuf;
00461   WorldState* ext=pose.getLoadedSensors();
00462 
00463   unsigned int newFrameNumber=(lastState!=NULL?lastState->frameNumber:frameNumber)+NumFrames;
00464 
00465   if(frameNumber>=newFrameNumber) {
00466     return; //sensors have already been filled in
00467   }
00468 
00469   if(lastState!=NULL && lastState!=this) {
00470     // copy over previous value if weight is 0
00471     //printf("this=%p, last=%p, ext=%p\n",this,lastState,ext);
00472     for(unsigned int i=0; i<NumOutputs; i++) {
00473       if (pose(i).weight>0) {
00474         if (i>=PIDJointOffset && i<PIDJointOffset+NumPIDJoints) {
00475           outputs[i] = pose(i).value / (config->motion.calibration_scale[i-PIDJointOffset])
00476             - config->motion.calibration_offset[i-PIDJointOffset];
00477         } else {
00478           outputs[i] = pose(i).value;
00479         }
00480       } else {
00481         outputs[i] = lastState->outputs[i];
00482       }
00483     }
00484   } else {
00485     // just ignore 0 weighted values
00486     for(unsigned int i=0; i<NumOutputs; i++) {
00487       if(pose(i).weight>0) {
00488         outputs[i]=pose(i).value;
00489       }
00490     }
00491     applyCalibration();
00492   }
00493 
00494   
00495   if(lastState!=NULL && lastState!=this) {
00496     alwaysGenerateStatus=lastState->alwaysGenerateStatus;
00497     //pid and velocity values will be copied at "completion"
00498     robotStatus=lastState->robotStatus;
00499     batteryStatus=lastState->batteryStatus;
00500     for(unsigned int i=0; i<PowerSrcID::NumPowerSIDs; i++)
00501       powerFlags[i]=lastState->powerFlags[i];
00502 
00503     //important part -- copy over button_times before calls to chkEvent
00504     for(unsigned int i=0; i<NumButtons; i++)
00505       button_times[i]=lastState->button_times[i];
00506   }
00507   
00508   if(ext!=NULL) {
00509     if(ext==this) {
00510       chkEvent(evtBuf,lastState);
00511     } else {
00512       if(lastState!=NULL && lastState!=this) {
00513         for(unsigned int i=0; i<NumButtons; i++)
00514           buttons[i]=ext->buttons[i];
00515         chkEvent(evtBuf,lastState);
00516       } else {
00517         for(unsigned int i=0; i<NumButtons; i++)
00518           chkEvent(evtBuf,i,ext->buttons[i],buttonNames[i]);
00519       }
00520       for(unsigned int i=0; i<NumSensors; i++)
00521         sensors[i]=ext->sensors[i];
00522       for(unsigned int i=0; i<NumPIDJoints; i++)
00523         pidduties[i]=ext->pidduties[i];
00524     }
00525   } else if(lastState!=NULL && lastState!=this) {
00526     // no loaded sensors at all -- copy over previous values
00527     for(unsigned int i=0; i<NumButtons; i++)
00528       buttons[i]=lastState->buttons[i];
00529     for(unsigned int i=0; i<NumSensors; i++)
00530       sensors[i]=lastState->sensors[i];
00531     for(unsigned int i=0; i<NumPIDJoints; i++)
00532       pidduties[i]=lastState->pidduties[i];
00533   }
00534 
00535   //unsigned int dif=curtime-(lastState==NULL ? lastSensorUpdateTime : lastState->lastSensorUpdateTime);
00536   lastSensorUpdateTime=curtime;
00537   if(newFrameNumber!=-1U)
00538     frameNumber=newFrameNumber;
00539   framesProcessed=(lastState!=NULL?lastState->framesProcessed:framesProcessed)+1;
00540 
00541   for(unsigned int i=0; i<evtBuf.size(); i++)
00542     er->postEvent(evtBuf[i]);
00543 
00544   //this version of read doesn't post the sensor update event -- the caller should do that
00545   // (this variant is used by the simulator, different parts fill in different sections, the last one should trigger the event)
00546   //er->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSrcID::UpdatedSID",1);
00547 }
00548 
00549 #endif //platform-specific sensor updating
00550 
00551 void WorldState::chkEvent(std::vector<EventBase>& evtBuf, unsigned int sid, float newval, const char* name) {
00552   if(newval>=0.1) { //now on
00553     if(buttons[sid]<0.1) { //was off: activation
00554       cout << ProcessID::getIDStr() << " post activate" << endl;
00555       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,name,newval));
00556       button_times[sid]=curtime;
00557     } else if(alwaysGenerateStatus || buttons[sid]!=newval) { //already on - always or change? : status
00558       unsigned int dur=curtime-button_times[sid];
00559       cout << ProcessID::getIDStr() << " post status" << endl;
00560       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,name,newval));
00561     }
00562   } else { //now off
00563     if(buttons[sid]>=0.1) { //was on: deactivation
00564       unsigned int dur=curtime-button_times[sid];
00565       button_times[sid]=0;
00566       cout << ProcessID::getIDStr() << " post deactivate" << endl;
00567       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,name,0));
00568     }
00569   }
00570   //update value
00571   buttons[sid]=newval;
00572 }
00573 
00574 void WorldState::chkEvent(std::vector<EventBase>& evtBuf, WorldState* lastState) {
00575   // posture already loaded data into this instance of WorldState
00576   // variation on chkEvent -- instead, we already hold the latest data, need to compare against lastState
00577   for(unsigned int sid=0; sid<NumButtons; sid++) {
00578     if(buttons[sid]>=0.1) { //now on
00579       if(lastState->buttons[sid]<0.1) { //was off: activation
00580         evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,buttonNames[sid],buttons[sid]));
00581         button_times[sid]=curtime;
00582       } else if(alwaysGenerateStatus || lastState->buttons[sid]!=buttons[sid]) { //already on - always or change? : status
00583         unsigned int dur=curtime-button_times[sid];
00584         evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,buttonNames[sid],buttons[sid]));
00585       }
00586     } else { //now off
00587       if(lastState->buttons[sid]>=0.1) { //was on: deactivation
00588         unsigned int dur=curtime-button_times[sid];
00589         button_times[sid]=0;
00590         evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,buttonNames[sid],0));
00591       }
00592     }
00593   }
00594 }
00595 
00596 void WorldState::applyCalibration() {
00597   for (unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++) {
00598     outputs[i] = (outputs[i]/ (config->motion.calibration_scale[i-PIDJointOffset]) ) - config->motion.calibration_offset[i-PIDJointOffset];
00599   }
00600 }
00601 
00602 /*! @file
00603  * @brief Implements WorldState, maintains information about the robot's environment, namely sensors and power status
00604  * @author ejt (Creator)
00605  *
00606  * $Author: kcomer $
00607  * $Name: tekkotsu-4_0 $
00608  * $Revision: 1.51 $
00609  * $State: Exp $
00610  * $Date: 2007/11/15 21:33:04 $
00611  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:57 2007 by Doxygen 1.5.4