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 "Events/EventBase.h"
00005 #include "ERS210Info.h"
00006 #include "ERS220Info.h"
00007 #include "ERS7Info.h"
00008 #include "Shared/Config.h"
00009 
00010 #ifdef PLATFORM_APERIOS
00011 #  include <OPENR/core_macro.h>
00012 #  include <OPENR/ObjcommTypes.h>
00013 #  include <OPENR/OPENR.h>
00014 #  include <OPENR/OPENRAPI.h>
00015 #  include <OPENR/OPENRMessages.h>
00016 #  include <OPENR/OPower.h>
00017 #else
00018 #  include "local/DataSource.h"
00019 #endif
00020 
00021 using namespace std;
00022 
00023 #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
00024 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value) //!< returns value from OPEN-R, as bool
00025 #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value) //!< return value from OPEN-R, as int
00026 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal) //!< returns signal from OPEN-R as word
00027 #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)
00028 
00029 const double WorldState::g=9.80665;
00030 const double WorldState::IROORDist = 900.0;
00031 
00032 #ifdef PLATFORM_APERIOS
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(0), 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, 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   evtBuf.reserve(NumButtons);
00072   unsigned int lastFrame=sensor.GetInfo(0)->numFrames-1;
00073 
00074   if(RobotName == ERS210Info::TargetName) {
00075     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointLFRotator);
00076     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointLFElevator);
00077     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointLFKnee);
00078     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLFRotator);
00079     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLFElevator);
00080     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointLFKnee);
00081   
00082     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointRFRotator);
00083     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointRFElevator);
00084     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointRFKnee);
00085     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRFRotator);
00086     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRFElevator);
00087     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointRFKnee);
00088 
00089     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointLHRotator);
00090     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointLHElevator);
00091     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointLHKnee);
00092     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLHRotator);
00093     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLHElevator);
00094     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointLHKnee);
00095 
00096     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS210Info::CPCJointRHRotator);
00097     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS210Info::CPCJointRHElevator);
00098     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS210Info::CPCJointRHKnee);
00099     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRHRotator);
00100     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRHElevator);
00101     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS210Info::CPCJointRHKnee);
00102 
00103     // Get head tilt,pan,roll joint angles
00104     outputs[HeadOffset+TiltOffset] = GETD(ERS210Info::CPCJointNeckTilt);
00105     outputs[HeadOffset+PanOffset ] = GETD(ERS210Info::CPCJointNeckPan);
00106     outputs[HeadOffset+RollOffset] = GETD(ERS210Info::CPCJointNeckRoll);
00107     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointNeckTilt);
00108     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS210Info::CPCJointNeckPan);
00109     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS210Info::CPCJointNeckRoll);
00110 
00111 #ifdef TGT_ERS210
00112     unsigned tail = ERS210Info::TailOffset;
00113     unsigned mouth = ERS210Info::MouthOffset;
00114 #elif defined(TGT_ERS2xx)
00115     unsigned tail = ERS2xxInfo::TailOffset;
00116     unsigned mouth = ERS2xxInfo::MouthOffset;
00117 #else
00118     unsigned tail = capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TailOffset]);
00119     unsigned mouth = capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::MouthOffset]);
00120 #endif
00121     outputs[tail+TiltOffset] = GETD(ERS210Info::CPCJointTailTilt);
00122     outputs[tail+PanOffset]  = GETD(ERS210Info::CPCJointTailPan);
00123     pidduties[tail+TiltOffset] = GETDUTY(ERS210Info::CPCJointTailTilt);
00124     pidduties[tail+PanOffset]  = GETDUTY(ERS210Info::CPCJointTailPan);
00125     
00126     outputs[mouth] = GETD(ERS210Info::CPCJointMouth);
00127     pidduties[mouth] = GETDUTY(ERS210Info::CPCJointMouth);
00128 
00129     // Get foot switches
00130     chkEvent(evtBuf,ERS210Info::LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),buttonNames[ERS210Info::LFrPawOffset]);
00131     chkEvent(evtBuf,ERS210Info::RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),buttonNames[ERS210Info::RFrPawOffset]);
00132     chkEvent(evtBuf,ERS210Info::LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),buttonNames[ERS210Info::LBkPawOffset]);
00133     chkEvent(evtBuf,ERS210Info::RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),buttonNames[ERS210Info::RBkPawOffset]);
00134 
00135     // Get buttons
00136     chkEvent(evtBuf,ERS210Info::ChinButOffset,  GETB(ERS210Info::CPCSensorChinSwitch),buttonNames[ERS210Info::ChinButOffset]);
00137     chkEvent(evtBuf,ERS210Info::BackButOffset,  GETB(ERS210Info::CPCSensorBackSwitch),buttonNames[ERS210Info::BackButOffset]);
00138     chkEvent(evtBuf,ERS210Info::HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),buttonNames[ERS210Info::HeadFrButOffset]);
00139     chkEvent(evtBuf,ERS210Info::HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),buttonNames[ERS210Info::HeadBkButOffset]);
00140 
00141     // Get IR distance sensor
00142     sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f;
00143 
00144     // Get acceleration sensors
00145     sensors[BAccelOffset] = GETD(ERS210Info::CPCSensorAccelFB);
00146     sensors[LAccelOffset] = GETD(ERS210Info::CPCSensorAccelLR);
00147     sensors[DAccelOffset] = GETD(ERS210Info::CPCSensorAccelUD);
00148 
00149     sensors[ERS210Info::ThermoOffset] = GETD(ERS210Info::CPCSensorThermoSensor);
00150   }
00151 
00152   // (ERS-220 only)
00153   if(RobotName == ERS220Info::TargetName) {
00154     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLFRotator);
00155     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLFElevator);
00156     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLFKnee);
00157     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLFRotator);
00158     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLFElevator);
00159     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLFKnee);
00160   
00161     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRFRotator);
00162     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRFElevator);
00163     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRFKnee);
00164     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRFRotator);
00165     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRFElevator);
00166     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRFKnee);
00167   
00168     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLHRotator);
00169     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLHElevator);
00170     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLHKnee);
00171     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLHRotator);
00172     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLHElevator);
00173     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLHKnee);
00174 
00175     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRHRotator);
00176     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRHElevator);
00177     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRHKnee);
00178     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRHRotator);
00179     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRHElevator);
00180     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRHKnee);
00181 
00182     // Get head tilt,pan,roll joint angles
00183     outputs[HeadOffset+TiltOffset] = GETD(ERS220Info::CPCJointNeckTilt);
00184     outputs[HeadOffset+PanOffset ] = GETD(ERS220Info::CPCJointNeckPan);
00185     outputs[HeadOffset+RollOffset] = GETD(ERS220Info::CPCJointNeckRoll);
00186     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS220Info::CPCJointNeckTilt);
00187     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS220Info::CPCJointNeckPan);
00188     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS220Info::CPCJointNeckRoll);
00189 
00190     // Get foot switches
00191     chkEvent(evtBuf,ERS220Info::LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),buttonNames[ERS220Info::LFrPawOffset]);
00192     chkEvent(evtBuf,ERS220Info::RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),buttonNames[ERS220Info::RFrPawOffset]);
00193     chkEvent(evtBuf,ERS220Info::LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),buttonNames[ERS220Info::LBkPawOffset]);
00194     chkEvent(evtBuf,ERS220Info::RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),buttonNames[ERS220Info::RBkPawOffset]);
00195 
00196     // Get buttons
00197     chkEvent(evtBuf,ERS220Info::ChinButOffset,  GETB(ERS220Info::CPCSensorChinSwitch),buttonNames[ERS220Info::ChinButOffset]);
00198     chkEvent(evtBuf,ERS220Info::BackButOffset,  GETB(ERS220Info::CPCSensorBackSwitch),buttonNames[ERS220Info::BackButOffset]);
00199     chkEvent(evtBuf,ERS220Info::HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),buttonNames[ERS220Info::HeadFrButOffset]);
00200     chkEvent(evtBuf,ERS220Info::HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),buttonNames[ERS220Info::HeadBkButOffset]);
00201     chkEvent(evtBuf,ERS220Info::TailLeftButOffset, GETB(ERS220Info::CPCSensorTailLeftSwitch),  buttonNames[ERS220Info::TailLeftButOffset]);
00202     chkEvent(evtBuf,ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),buttonNames[ERS220Info::TailCenterButOffset]);
00203     chkEvent(evtBuf,ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), buttonNames[ERS220Info::TailRightButOffset]);
00204 
00205     // Get IR distance sensor
00206     sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f;
00207 
00208     // Get acceleration sensors
00209     sensors[BAccelOffset] = GETD(ERS220Info::CPCSensorAccelFB);
00210     sensors[LAccelOffset] = GETD(ERS220Info::CPCSensorAccelLR);
00211     sensors[DAccelOffset] = GETD(ERS220Info::CPCSensorAccelUD);
00212 
00213     sensors[ERS220Info::ThermoOffset] = GETD(ERS220Info::CPCSensorThermoSensor);
00214   }
00215 
00216   // (ERS-7 only)
00217   if(RobotName == ERS7Info::TargetName) {
00218     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointLFRotator);
00219     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointLFElevator);
00220     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointLFKnee);
00221     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLFRotator);
00222     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLFElevator);
00223     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointLFKnee);
00224   
00225     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointRFRotator);
00226     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointRFElevator);
00227     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointRFKnee);
00228     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRFRotator);
00229     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRFElevator);
00230     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointRFKnee);
00231   
00232     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointLHRotator);
00233     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointLHElevator);
00234     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointLHKnee);
00235     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLHRotator);
00236     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLHElevator);
00237     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointLHKnee);
00238 
00239     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS7Info::CPCJointRHRotator);
00240     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS7Info::CPCJointRHElevator);
00241     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS7Info::CPCJointRHKnee);
00242     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRHRotator);
00243     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRHElevator);
00244     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS7Info::CPCJointRHKnee);
00245 
00246     // Get head tilt,pan,nod joint angles
00247     outputs[HeadOffset+TiltOffset] = GETD(ERS7Info::CPCJointNeckTilt);
00248     outputs[HeadOffset+PanOffset ] = GETD(ERS7Info::CPCJointNeckPan);
00249     outputs[HeadOffset+RollOffset] = GETD(ERS7Info::CPCJointNeckNod);
00250     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointNeckTilt);
00251     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS7Info::CPCJointNeckPan);
00252     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS7Info::CPCJointNeckNod);
00253 
00254     outputs[ERS7Info::TailOffset+TiltOffset] = GETD(ERS7Info::CPCJointTailTilt);
00255     outputs[ERS7Info::TailOffset+PanOffset]  = GETD(ERS7Info::CPCJointTailPan);
00256     pidduties[ERS7Info::TailOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointTailTilt);
00257     pidduties[ERS7Info::TailOffset+PanOffset]  = GETDUTY(ERS7Info::CPCJointTailPan);
00258     
00259     outputs[ERS7Info::MouthOffset] = GETD(ERS7Info::CPCJointMouth);
00260     pidduties[ERS7Info::MouthOffset] = GETDUTY(ERS7Info::CPCJointMouth);
00261 
00262     // Get foot switches
00263     chkEvent(evtBuf,ERS7Info::LFrPawOffset,GETB(ERS7Info::CPCSwitchLFPaw),buttonNames[ERS7Info::LFrPawOffset]);
00264     chkEvent(evtBuf,ERS7Info::RFrPawOffset,GETB(ERS7Info::CPCSwitchRFPaw),buttonNames[ERS7Info::RFrPawOffset]);
00265     chkEvent(evtBuf,ERS7Info::LBkPawOffset,GETB(ERS7Info::CPCSwitchLHPaw),buttonNames[ERS7Info::LBkPawOffset]);
00266     chkEvent(evtBuf,ERS7Info::RBkPawOffset,GETB(ERS7Info::CPCSwitchRHPaw),buttonNames[ERS7Info::RBkPawOffset]);
00267 
00268     // Get buttons/switches
00269     // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210
00270     chkEvent(evtBuf, ERS7Info::ChinButOffset,       GETSENSOR(ERS7Info::CPCSwitchChin),      buttonNames[ERS7Info::ChinButOffset]);
00271     chkEvent(evtBuf, ERS7Info::HeadButOffset,       GETSENSOR(ERS7Info::CPCSensorHead)/120,      buttonNames[ERS7Info::HeadButOffset]);
00272     chkEvent(evtBuf, ERS7Info::FrontBackButOffset,  GETSENSOR(ERS7Info::CPCSensorBackFront)/150, buttonNames[ERS7Info::FrontBackButOffset]);
00273     chkEvent(evtBuf, ERS7Info::MiddleBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150,buttonNames[ERS7Info::MiddleBackButOffset]);
00274     chkEvent(evtBuf, ERS7Info::RearBackButOffset,   GETSENSOR(ERS7Info::CPCSensorBackRear)/150,  buttonNames[ERS7Info::RearBackButOffset]);
00275     chkEvent(evtBuf, ERS7Info::WirelessSwOffset,GETSENSOR(ERS7Info::CPCSwitchWireless),  buttonNames[ERS7Info::WirelessSwOffset]);
00276 
00277     // Get IR distance sensor
00278     sensors[ERS7Info::NearIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorNearPSD) / 1000.0f;
00279     sensors[ERS7Info::FarIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorFarPSD) / 1000.0f;
00280     sensors[ERS7Info::ChestIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorChestPSD) / 1000.0f;
00281 
00282     // Get acceleration sensors
00283     sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB);
00284     sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR);
00285     sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD);
00286   }
00287 
00288   //unsigned int dif=curtime-(lastState==NULL ? lastSensorUpdateTime : lastState->lastSensorUpdateTime);
00289   lastSensorUpdateTime=curtime;
00290   frameNumber=newFrameNumber;
00291   ++framesProcessed;
00292   
00293   //Apply sensor calibrations (currently only PID joint positions - perhaps sensors as well?)
00294   applyCalibration();
00295 
00296   for(unsigned int i=0; i<evtBuf.size(); i++)
00297     er->postEvent(evtBuf[i]);
00298 
00299   //this version of read doesn't post the sensor update event -- the caller should do that
00300   //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)
00301   //er->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1);
00302 }
00303 
00304 /*! This will cause events to be posted */
00305 void WorldState::read(const OPowerStatus& power, EventRouter* er) {
00306   std::string actnames[PowerSrcID::NumPowerSIDs];
00307   std::string denames[PowerSrcID::NumPowerSIDs];
00308   unsigned int actmasks[PowerSrcID::NumPowerSIDs];
00309   memset(actmasks,0,sizeof(unsigned int)*PowerSrcID::NumPowerSIDs);
00310 
00311   //RobotStatus
00312   chkPowerEvent(PowerSrcID::PauseSID,          power.robotStatus,orsbPAUSE,                        "Pause",actnames,denames,actmasks);
00313   chkPowerEvent(PowerSrcID::MotorPowerSID,     power.robotStatus,orsbMOTOR_POWER,                  "MotorPower",actnames,denames,actmasks);
00314   chkPowerEvent(PowerSrcID::VibrationSID,      power.robotStatus,orsbVIBRATION_DETECT,             "Vibration",actnames,denames,actmasks);
00315   chkPowerEvent(PowerSrcID::ExternalPortSID,   power.robotStatus,orsbEX_PORT_CONNECTED,            "ExternalPort",actnames,denames,actmasks);
00316   chkPowerEvent(PowerSrcID::StationConnectSID, power.robotStatus,orsbSTATION_CONNECTED,            "StationConnect",actnames,denames,actmasks);
00317   chkPowerEvent(PowerSrcID::ExternalPowerSID,  power.robotStatus,orsbEX_POWER_CONNECTED,           "ExternalPower",actnames,denames,actmasks);
00318   chkPowerEvent(PowerSrcID::BatteryConnectSID, power.robotStatus,orsbBATTERY_CONNECTED,            "BatteryConnect",actnames,denames,actmasks);
00319   chkPowerEvent(PowerSrcID::ChargingSID,       power.robotStatus,orsbBATTERY_CHARGING,             "BatteryCharging",actnames,denames,actmasks);
00320   chkPowerEvent(PowerSrcID::BatteryFullSID,    power.robotStatus,orsbBATTERY_CAPACITY_FULL,        "BatteryFull",actnames,denames,actmasks);
00321   chkPowerEvent(PowerSrcID::LowPowerWarnSID,   power.robotStatus,orsbBATTERY_CAPACITY_LOW,         "BatteryLow",actnames,denames,actmasks);
00322   chkPowerEvent(PowerSrcID::OverChargedSID,    power.robotStatus,orsbBATTERY_OVER_CURRENT,         "BatteryOverCurrent",actnames,denames,actmasks);
00323   chkPowerEvent(PowerSrcID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_DISCHARGING,"BatteryOverTempDischarge",actnames,denames,actmasks);
00324   chkPowerEvent(PowerSrcID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_CHARGING,   "BatteryOverTempCharge",actnames,denames,actmasks);
00325   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbBATTERY_ERROR_OF_CHARGING,    "BatteryChargeError",actnames,denames,actmasks);
00326   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbERROR_OF_PLUNGER,             "PlungerError",actnames,denames,actmasks);
00327   chkPowerEvent(PowerSrcID::PowerGoodSID,      power.robotStatus,orsbOPEN_R_POWER_GOOD,            "PowerGood",actnames,denames,actmasks);
00328   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbERROR_OF_FAN,                 "FanError",actnames,denames,actmasks);
00329   chkPowerEvent(PowerSrcID::DataFromStationSID,power.robotStatus,orsbDATA_STREAM_FROM_STATION,     "DataFromStation",actnames,denames,actmasks);
00330   chkPowerEvent(PowerSrcID::RegisterUpdateSID, power.robotStatus,orsbREGISTER_UPDATED_BY_STATION,  "RegisterUpdate",actnames,denames,actmasks);
00331   chkPowerEvent(PowerSrcID::ErrorSID,          power.robotStatus,orsbRTC_ERROR,                    "RTCError",actnames,denames,actmasks);
00332   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_OVERFLOW,                 "RTCOverflow",actnames,denames,actmasks);
00333   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_RESET,                    "RTCReset",actnames,denames,actmasks);
00334   chkPowerEvent(PowerSrcID::RTCSID,            power.robotStatus,orsbRTC_SET,                      "RTCSet",actnames,denames,actmasks);
00335   chkPowerEvent(PowerSrcID::SpecialModeSID,    power.robotStatus,orsbSPECIAL_MODE,                 "SpecialMode",actnames,denames,actmasks);
00336   chkPowerEvent(PowerSrcID::BMNDebugModeSID,   power.robotStatus,orsbBMN_DEBUG_MODE,               "BMNDebugMode",actnames,denames,actmasks);
00337   chkPowerEvent(PowerSrcID::ChargerStatusSID,  power.robotStatus,orsbCHARGER_STATUS,               "ChargerStatus",actnames,denames,actmasks);
00338   chkPowerEvent(PowerSrcID::PlungerSID,        power.robotStatus,orsbPLUNGER,                      "Plunger",actnames,denames,actmasks);
00339   chkPowerEvent(PowerSrcID::SuspendedSID,      power.robotStatus,orsbSUSPENDED,                    "Suspended",actnames,denames,actmasks);
00340 
00341   //BatteryStatus
00342   chkPowerEvent(PowerSrcID::ErrorSID,        power.batteryStatus,obsbERROR_CODE_MASK,             "BatteryError",actnames,denames,actmasks);
00343   chkPowerEvent(PowerSrcID::BatteryEmptySID, power.batteryStatus,obsbFULLY_DISCHARGED,            "FullyDischarged",actnames,denames,actmasks);
00344   chkPowerEvent(PowerSrcID::BatteryFullSID,  power.batteryStatus,obsbFULLY_CHARGED,               "FullyCharged",actnames,denames,actmasks);
00345   chkPowerEvent(PowerSrcID::DischargingSID,  power.batteryStatus,obsbDISCHARGING,                 "Discharging",actnames,denames,actmasks);
00346   chkPowerEvent(PowerSrcID::BatteryInitSID,  power.batteryStatus,obsbINITIALIZED,                 "BatteryInit",actnames,denames,actmasks);
00347   chkPowerEvent(PowerSrcID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_TIME_ALARM,        "RemainingTimeAlarm",actnames,denames,actmasks);
00348   chkPowerEvent(PowerSrcID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_CAPACITY_ALARM,    "RemainingCapacityAlarm",actnames,denames,actmasks);
00349   chkPowerEvent(PowerSrcID::TermDischargeSID,power.batteryStatus,obsbTERMINATED_DISCHARGING_ALARM,"TermDischargeAlarm",actnames,denames,actmasks);
00350   chkPowerEvent(PowerSrcID::OverheatingSID,  power.batteryStatus,obsbOVER_TEMP_ALARM,             "OverTempAlarm",actnames,denames,actmasks);
00351   chkPowerEvent(PowerSrcID::TermChargeSID,   power.batteryStatus,obsbTERMINATED_CHARGING_ALARM,   "TermChargeAlarm",actnames,denames,actmasks);
00352   chkPowerEvent(PowerSrcID::OverChargedSID,  power.batteryStatus,obsbOVER_CHARGED_ALARM,          "OverChargeAlarm",actnames,denames,actmasks);
00353   
00354   sensors[PowerRemainOffset] = power.remainingCapacity/100.0;
00355   sensors[PowerThermoOffset] = power.temperature/100.0;
00356   sensors[PowerCapacityOffset] = power.fullyChargedCapacity;
00357   sensors[PowerVoltageOffset] = power.voltage/1000.0;
00358   sensors[PowerCurrentOffset] = power.current;
00359 
00360   //only generate status events when a change happens
00361   for(unsigned int i=0; i<PowerSrcID::NumPowerSIDs; i++) {
00362     if(actmasks[i]) { //now on
00363       if(!powerFlags[i]) //was off: activation
00364         er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00365       else if(actmasks[i]!=powerFlags[i]) //already on - change? : status
00366         er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00367     } else { // now off
00368       if(powerFlags[i]) //was on: deactivation
00369         er->postEvent(EventBase::powerEGID,i,EventBase::deactivateETID,0,denames[i],0);
00370     }
00371     powerFlags[i]=actmasks[i];
00372   }
00373 
00374   er->postEvent(EventBase::powerEGID,PowerSrcID::UpdatedSID,EventBase::statusETID,0,"PowerSrcID::UpdatedSID",1);
00375 }
00376 
00377 #else // PLATFORM_LOCAL
00378 
00379 void WorldState::read(const SensorState& sensor, bool sendEvents) {
00380   ++framesProcessed;
00381   
00382   unsigned int dif = sensor.timestamp - lastSensorUpdateTime;
00383   
00384   // motion thread polls for updates, which might not actually be any, so only assert if !sendEvents
00385   // if sending events, there definitely should be an increment in frame number
00386   ASSERT(!sendEvents || frameNumber!=sensor.frameNumber,"duplicate sensor frame processing, old " << frameNumber << " at " << get_time() << " (stamp=" << sensor.timestamp <<")");
00387   
00388   if(frameNumber!=sensor.frameNumber || lastSensorUpdateTime!=sensor.timestamp) {
00389     ASSERT(frameNumber<=sensor.frameNumber,"frameNumbers running in reverse, old " << frameNumber << " new " << sensor.frameNumber << " at " << get_time() << " (stamp=" << sensor.timestamp <<")");
00390     ASSERT(lastSensorUpdateTime<=sensor.timestamp,"timestamps running in reverse, old " << lastSensorUpdateTime << " new " << sensor.timestamp << " at " << get_time());
00391 
00392     frameNumber=sensor.frameNumber;
00393     curtime = lastSensorUpdateTime = sensor.timestamp;
00394     
00395     for(unsigned int i=0; i<PIDJointOffset; i++)
00396       outputs[i] = sensor.outputs[i];
00397     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++) {
00398       float calScale = config->motion.calibration_scale[i-PIDJointOffset];
00399       float calOffset = config->motion.calibration_offset[i-PIDJointOffset];
00400       outputs[i] = sensor.outputs[i] / calScale - calOffset;
00401     }
00402     for(unsigned int i=PIDJointOffset+NumPIDJoints; i<NumOutputs; i++)
00403       outputs[i] = sensor.outputs[i];
00404     
00405     if(!sendEvents) {
00406       // first set button_times properly:
00407       for(unsigned int i=0; i<NumButtons; i++) {
00408         if(sensor.buttons[i]>=0.1) {
00409           if(buttons[i]<0.1)
00410             button_times[i]=curtime;
00411         } else {
00412           if(buttons[i]>=0.1)
00413             button_times[i]=0;
00414         }
00415       }
00416       // this copies buttons, sensors, pids[][], and pidduties in one go:
00417       // (may eventually want to split sensors off to apply calibration factors...)
00418       memcpy(buttons,sensor.buttons,sizeof(float)*(NumButtons + NumSensors + NumPIDJoints*3 + NumPIDJoints));
00419     } else {
00420       std::vector<EventBase> evtBuf;
00421       evtBuf.reserve(NumButtons);
00422       for(unsigned int i=0; i<NumButtons; i++)
00423         chkEvent(evtBuf,i,sensor.buttons[i],buttonNames[i]);
00424       
00425       // this copies sensors, pids[][], and pidduties in one go:
00426       // (may eventually want to split sensors off to apply calibration factors...)
00427       memcpy(sensors,sensor.sensors,sizeof(float)*(NumSensors + NumPIDJoints*3 + NumPIDJoints));
00428       for(unsigned int i=0; i<evtBuf.size(); i++)
00429         erouter->postEvent(evtBuf[i]);
00430     }
00431   }
00432   
00433   if(sendEvents)
00434     erouter->postEvent(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1);
00435 }
00436 
00437 #endif //platform-specific sensor updating
00438 
00439 void WorldState::chkEvent(std::vector<EventBase>& evtBuf, unsigned int sid, float newval, const char* name) {
00440   if(newval>=0.1) { //now on
00441     if(buttons[sid]<0.1) { //was off: activation
00442       //cout << ProcessID::getIDStr() << " post activate button " << name << endl;
00443       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,name,newval));
00444       button_times[sid]=curtime;
00445     } else if(alwaysGenerateStatus || buttons[sid]!=newval) { //already on - always or change? : status
00446       unsigned int dur=curtime-button_times[sid];
00447       //cout << ProcessID::getIDStr() << " post status" << endl;
00448       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,name,newval));
00449     }
00450   } else { //now off
00451     if(buttons[sid]>=0.1) { //was on: deactivation
00452       unsigned int dur=curtime-button_times[sid];
00453       button_times[sid]=0;
00454       //cout << ProcessID::getIDStr() << " post deactivate" << endl;
00455       evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,name,0));
00456     }
00457   }
00458   //update value
00459   buttons[sid]=newval;
00460 }
00461 
00462 void WorldState::applyCalibration() {
00463   for (unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++) {
00464     float calScale = config->motion.calibration_scale[i-PIDJointOffset];
00465     float calOffset = config->motion.calibration_offset[i-PIDJointOffset];
00466     outputs[i] = outputs[i] / calScale - calOffset;
00467   }
00468 }
00469 
00470 /*! @file
00471  * @brief Implements WorldState, maintains information about the robot's environment, namely sensors and power status
00472  * @author ejt (Creator)
00473  */

Tekkotsu v5.1CVS
 		Generated Mon May 9 04:58:53 2016 by Doxygen 1.6.3