Tekkotsu: WorldState.cc Source 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 +