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)
00023 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value)
00024 #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value)
00025 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal)
00026 #define GETDUTY(cpc) (((OJointValue*)(void*)&sensor.GetData(cpc)->frame[lastFrame])->pwmDuty/512.0f)
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
00062 void WorldState::read(OSensorFrameVectorData& sensor, WorldState* lastState, EventRouter* er) {
00063
00064 unsigned int newFrameNumber=sensor.GetInfo(0)->frameNumber;
00065 if(frameNumber>=newFrameNumber)
00066 return;
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
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
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
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
00143
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
00155 chkEvent(evtBuf,lastState);
00156 } else {
00157
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
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
00171 sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f;
00172
00173
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
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
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
00221
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
00236 chkEvent(evtBuf,lastState);
00237 } else {
00238
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
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
00255 sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f;
00256
00257
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
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
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
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
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
00327 chkEvent(evtBuf,lastState);
00328 } else {
00329
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
00336
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
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
00351 sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB);
00352 sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR);
00353 sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD);
00354 }
00355
00356
00357 lastSensorUpdateTime=curtime;
00358 frameNumber=newFrameNumber;
00359 framesProcessed=(lastState!=NULL?lastState->framesProcessed:framesProcessed)+1;
00360
00361
00362
00363
00364
00365
00366 applyCalibration();
00367
00368 for(unsigned int i=0; i<evtBuf.size(); i++)
00369 er->postEvent(evtBuf[i]);
00370
00371
00372
00373
00374 }
00375
00376
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
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
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
00433 for(unsigned int i=0; i<PowerSrcID::NumPowerSIDs; i++) {
00434 if(actmasks[i]) {
00435 if(!powerFlags[i])
00436 er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00437 else if(actmasks[i]!=powerFlags[i])
00438 er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00439 } else {
00440 if(powerFlags[i])
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
00452
00453
00454
00455
00456
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;
00467 }
00468
00469 if(lastState!=NULL && lastState!=this) {
00470
00471
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
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
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
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
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
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
00545
00546
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) {
00553 if(buttons[sid]<0.1) {
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) {
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 {
00563 if(buttons[sid]>=0.1) {
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
00571 buttons[sid]=newval;
00572 }
00573
00574 void WorldState::chkEvent(std::vector<EventBase>& evtBuf, WorldState* lastState) {
00575
00576
00577 for(unsigned int sid=0; sid<NumButtons; sid++) {
00578 if(buttons[sid]>=0.1) {
00579 if(lastState->buttons[sid]<0.1) {
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]) {
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 {
00587 if(lastState->buttons[sid]>=0.1) {
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
00603
00604
00605
00606
00607
00608
00609
00610
00611