00001 #ifdef PLATFORM_APERIOS
00002 #include <OPENR/core_macro.h>
00003 #include <OPENR/ObjcommTypes.h>
00004 #include <OPENR/OPENR.h>
00005 #include <OPENR/OPENRAPI.h>
00006 #include <OPENR/OPENRMessages.h>
00007 #include <OPENR/OPower.h>
00008 #endif
00009
00010 #include "WorldState.h"
00011 #include "Shared/get_time.h"
00012 #include "Events/EventRouter.h"
00013 #include "ERS210Info.h"
00014 #include "ERS220Info.h"
00015 #include "ERS7Info.h"
00016 #include "Shared/Config.h"
00017
00018 #define GETD(cpc) (((float)sensor.GetData(cpc)->frame[lastFrame].value) / 1.0E6f)
00019 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value)
00020 #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value)
00021 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal)
00022 #define GETDUTY(cpc) ((float)((OJointValue*)&sensor.GetData(cpc)->frame[lastFrame])->pwmDuty/512.0f)
00023
00024 const double WorldState::g=9.80665;
00025 const double WorldState::IROORDist = 900.0;
00026
00027 WorldState * state=NULL;
00028
00029 WorldState::WorldState()
00030 : alwaysGenerateStatus(false), vel_x(0), vel_y(0), vel_a(0), vel_time(0),
00031 robotStatus(0), batteryStatus(0),
00032 lastSensorUpdateTime(0), mainProfile(), motionProfile(),
00033 robotDesign(0), curtime(get_time())
00034 {
00035 for(unsigned int i=0; i< NumOutputs; i++)
00036 outputs[i]=0;
00037 for(unsigned int i=0; i< NumButtons; i++)
00038 buttons[i]=0;
00039 for(unsigned int i=0; i< NumSensors; i++)
00040 sensors[i]=0;
00041 for(unsigned int i=0; i< NumPIDJoints; i++)
00042 for(unsigned int j=0; j<3; j++)
00043 pids[i][j]=0;
00044 for(unsigned int i=0; i< NumPIDJoints; i++)
00045 pidduties[i]=0;
00046 memset(powerFlags,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs);
00047 memset(button_times,0,sizeof(unsigned int)*NumButtons);
00048
00049 #ifdef PLATFORM_APERIOS
00050
00051 char robotDesignStr[orobotdesignNAME_MAX + 1];
00052 memset(robotDesignStr, 0, sizeof(robotDesignStr));
00053 if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) {
00054 cout << "OPENR::GetRobotDesign() failed." << endl;
00055 } else {
00056 if(strcmp(robotDesignStr,"ERS-210")==0)
00057 robotDesign=ERS210Mask;
00058 else if(strcmp(robotDesignStr,"ERS-220")==0)
00059 robotDesign=ERS220Mask;
00060 else if(strcmp(robotDesignStr,"ERS-7")==0)
00061 robotDesign=ERS7Mask;
00062 else {
00063 cout << "ERROR: Unrecognized model: "<<robotDesignStr<<"\nDoing the best I can by assuming ERS-7..."<<endl;
00064 robotDesign=ERS7Mask;
00065 }
00066 }
00067 #else
00068
00069
00070 # if TGT_ERS220
00071 robotDesign=ERS220Mask;
00072 # elif TGT_ERS210
00073 robotDesign=ERS210Mask;
00074 # elif TGT_ERS2xx
00075 # warning "TGT_ERS2xx can't be determined on non-Aperios - defaulting to TGT_ERS210"
00076 robotDesign=ERS210Mask;
00077 # elif TGT_ERS7
00078 robotDesign=ERS7Mask;
00079 # else
00080 # warning "TGT_<model> undefined - defaulting to TGT_ERS7"
00081 robotDesign=ERS7Mask;
00082 # endif //model selection
00083 #endif
00084 }
00085
00086 #ifdef PLATFORM_APERIOS
00087
00088
00089
00090 void WorldState::read(OSensorFrameVectorData& sensor, EventRouter* er) {
00091 curtime=get_time();
00092
00093 std::vector<EventBase*> evtBuf;
00094 unsigned int lastFrame=sensor.GetInfo(0)->numFrames-1;
00095
00096 if(robotDesign&ERS210Mask) {
00097 outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointLFRotator);
00098 outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointLFElevator);
00099 outputs[LFrLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointLFKnee);
00100 pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLFRotator);
00101 pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLFElevator);
00102 pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointLFKnee);
00103
00104 outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointRFRotator);
00105 outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointRFElevator);
00106 outputs[RFrLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointRFKnee);
00107 pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRFRotator);
00108 pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRFElevator);
00109 pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointRFKnee);
00110
00111 outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointLHRotator);
00112 outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointLHElevator);
00113 outputs[LBkLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointLHKnee);
00114 pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLHRotator);
00115 pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLHElevator);
00116 pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointLHKnee);
00117
00118 outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointRHRotator);
00119 outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointRHElevator);
00120 outputs[RBkLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointRHKnee);
00121 pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRHRotator);
00122 pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRHElevator);
00123 pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointRHKnee);
00124
00125
00126 outputs[HeadOffset+TiltOffset] = GETD(ERS210Info::CPCJointNeckTilt);
00127 outputs[HeadOffset+PanOffset ] = GETD(ERS210Info::CPCJointNeckPan);
00128 outputs[HeadOffset+RollOffset] = GETD(ERS210Info::CPCJointNeckRoll);
00129 pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointNeckTilt);
00130 pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS210Info::CPCJointNeckPan);
00131 pidduties[HeadOffset+RollOffset] = GETDUTY(ERS210Info::CPCJointNeckRoll);
00132
00133 outputs[ERS210Info::TailOffset+TiltOffset] = GETD(ERS210Info::CPCJointTailTilt);
00134 outputs[ERS210Info::TailOffset+PanOffset] = GETD(ERS210Info::CPCJointTailPan);
00135 pidduties[ERS210Info::TailOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointTailTilt);
00136 pidduties[ERS210Info::TailOffset+PanOffset] = GETDUTY(ERS210Info::CPCJointTailPan);
00137
00138 outputs[ERS210Info::MouthOffset] = GETD(ERS210Info::CPCJointMouth);
00139 pidduties[ERS210Info::MouthOffset] = GETDUTY(ERS210Info::CPCJointMouth);
00140
00141
00142 chkEvent(evtBuf,LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),"LFrPaw");
00143 chkEvent(evtBuf,RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),"RFrPaw");
00144 chkEvent(evtBuf,LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),"LBkPaw");
00145 chkEvent(evtBuf,RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),"RBkPaw");
00146
00147
00148 chkEvent(evtBuf,ERS210Info::ChinButOffset, GETB(ERS210Info::CPCSensorChinSwitch),"ChinBut");
00149 chkEvent(evtBuf,ERS210Info::BackButOffset, GETB(ERS210Info::CPCSensorBackSwitch),"BackBut");
00150 chkEvent(evtBuf,ERS210Info::HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),"HeadFrBut");
00151 chkEvent(evtBuf,ERS210Info::HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),"HeadBkBut");
00152
00153
00154 sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f;
00155
00156
00157 sensors[BAccelOffset] = GETD(ERS210Info::CPCSensorAccelFB);
00158 sensors[LAccelOffset] = GETD(ERS210Info::CPCSensorAccelLR);
00159 sensors[DAccelOffset] = GETD(ERS210Info::CPCSensorAccelUD);
00160
00161 sensors[ERS210Info::ThermoOffset] = GETD(ERS210Info::CPCSensorThermoSensor);
00162 }
00163
00164
00165 if(robotDesign&ERS220Mask) {
00166 outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointLFRotator);
00167 outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointLFElevator);
00168 outputs[LFrLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointLFKnee);
00169 pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLFRotator);
00170 pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLFElevator);
00171 pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointLFKnee);
00172
00173 outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointRFRotator);
00174 outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointRFElevator);
00175 outputs[RFrLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointRFKnee);
00176 pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRFRotator);
00177 pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRFElevator);
00178 pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointRFKnee);
00179
00180 outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointLHRotator);
00181 outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointLHElevator);
00182 outputs[LBkLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointLHKnee);
00183 pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLHRotator);
00184 pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLHElevator);
00185 pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointLHKnee);
00186
00187 outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointRHRotator);
00188 outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointRHElevator);
00189 outputs[RBkLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointRHKnee);
00190 pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRHRotator);
00191 pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRHElevator);
00192 pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointRHKnee);
00193
00194
00195 outputs[HeadOffset+TiltOffset] = GETD(ERS220Info::CPCJointNeckTilt);
00196 outputs[HeadOffset+PanOffset ] = GETD(ERS220Info::CPCJointNeckPan);
00197 outputs[HeadOffset+RollOffset] = GETD(ERS220Info::CPCJointNeckRoll);
00198 pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS220Info::CPCJointNeckTilt);
00199 pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS220Info::CPCJointNeckPan);
00200 pidduties[HeadOffset+RollOffset] = GETDUTY(ERS220Info::CPCJointNeckRoll);
00201
00202 chkEvent(evtBuf, ERS220Info::TailLeftButOffset, GETB(ERS220Info::CPCSensorTailLeftSwitch), "LBkBut");
00203 chkEvent(evtBuf, ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),"CBkBut");
00204 chkEvent(evtBuf, ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), "RBkBut");
00205
00206
00207 chkEvent(evtBuf,LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),"LFrPaw");
00208 chkEvent(evtBuf,RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),"RFrPaw");
00209 chkEvent(evtBuf,LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),"LBkPaw");
00210 chkEvent(evtBuf,RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),"RBkPaw");
00211
00212
00213 chkEvent(evtBuf,ERS220Info::ChinButOffset, GETB(ERS220Info::CPCSensorChinSwitch),"ChinBut");
00214 chkEvent(evtBuf,ERS220Info::BackButOffset, GETB(ERS220Info::CPCSensorBackSwitch),"BackBut");
00215 chkEvent(evtBuf,ERS220Info::HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),"HeadFrBut");
00216 chkEvent(evtBuf,ERS220Info::HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),"HeadBkBut");
00217
00218
00219 sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f;
00220
00221
00222 sensors[BAccelOffset] = GETD(ERS220Info::CPCSensorAccelFB);
00223 sensors[LAccelOffset] = GETD(ERS220Info::CPCSensorAccelLR);
00224 sensors[DAccelOffset] = GETD(ERS220Info::CPCSensorAccelUD);
00225
00226 sensors[ERS220Info::ThermoOffset] = GETD(ERS220Info::CPCSensorThermoSensor);
00227 }
00228
00229
00230 if(robotDesign&ERS7Mask) {
00231 outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointLFRotator);
00232 outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointLFElevator);
00233 outputs[LFrLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointLFKnee);
00234 pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLFRotator);
00235 pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLFElevator);
00236 pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointLFKnee);
00237
00238 outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointRFRotator);
00239 outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointRFElevator);
00240 outputs[RFrLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointRFKnee);
00241 pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRFRotator);
00242 pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRFElevator);
00243 pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointRFKnee);
00244
00245 outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointLHRotator);
00246 outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointLHElevator);
00247 outputs[LBkLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointLHKnee);
00248 pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLHRotator);
00249 pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLHElevator);
00250 pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointLHKnee);
00251
00252 outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointRHRotator);
00253 outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointRHElevator);
00254 outputs[RBkLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointRHKnee);
00255 pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRHRotator);
00256 pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRHElevator);
00257 pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointRHKnee);
00258
00259
00260 outputs[HeadOffset+TiltOffset] = GETD(ERS7Info::CPCJointNeckTilt);
00261 outputs[HeadOffset+PanOffset ] = GETD(ERS7Info::CPCJointNeckPan);
00262 outputs[HeadOffset+RollOffset] = GETD(ERS7Info::CPCJointNeckNod);
00263 pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointNeckTilt);
00264 pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS7Info::CPCJointNeckPan);
00265 pidduties[HeadOffset+RollOffset] = GETDUTY(ERS7Info::CPCJointNeckNod);
00266
00267 outputs[ERS7Info::TailOffset+TiltOffset] = GETD(ERS7Info::CPCJointTailTilt);
00268 outputs[ERS7Info::TailOffset+PanOffset] = GETD(ERS7Info::CPCJointTailPan);
00269 pidduties[ERS7Info::TailOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointTailTilt);
00270 pidduties[ERS7Info::TailOffset+PanOffset] = GETDUTY(ERS7Info::CPCJointTailPan);
00271
00272 outputs[ERS7Info::MouthOffset] = GETD(ERS7Info::CPCJointMouth);
00273 pidduties[ERS7Info::MouthOffset] = GETDUTY(ERS7Info::CPCJointMouth);
00274
00275
00276 chkEvent(evtBuf,LFrPawOffset,GETB(ERS7Info::CPCSwitchLFPaw),"LFrPaw");
00277 chkEvent(evtBuf,RFrPawOffset,GETB(ERS7Info::CPCSwitchRFPaw),"RFrPaw");
00278 chkEvent(evtBuf,LBkPawOffset,GETB(ERS7Info::CPCSwitchLHPaw),"LBkPaw");
00279 chkEvent(evtBuf,RBkPawOffset,GETB(ERS7Info::CPCSwitchRHPaw),"RBkPaw");
00280
00281
00282
00283 chkEvent(evtBuf, ERS7Info::ChinButOffset, GETSENSOR(ERS7Info::CPCSwitchChin), "ChinBut");
00284 chkEvent(evtBuf, ERS7Info::HeadButOffset, GETSENSOR(ERS7Info::CPCSensorHead)/120, "HeadBut");
00285 chkEvent(evtBuf, ERS7Info::FrontBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackFront)/150, "BkFrBut");
00286 chkEvent(evtBuf, ERS7Info::MiddleBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150,"BkMdBut");
00287 chkEvent(evtBuf, ERS7Info::RearBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackRear)/150, "BkRrBut");
00288 chkEvent(evtBuf, ERS7Info::WirelessSwOffset,GETSENSOR(ERS7Info::CPCSwitchWireless), "WirelessSw");
00289
00290
00291 sensors[ERS7Info::NearIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorNearPSD) / 1000.0f;
00292 sensors[ERS7Info::FarIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorFarPSD) / 1000.0f;
00293 sensors[ERS7Info::ChestIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorChestPSD) / 1000.0f;
00294
00295
00296 sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB);
00297 sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR);
00298 sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD);
00299 }
00300
00301
00302 for(unsigned int i=0; i<NumPIDJoints; i++)
00303 outputs[PIDJointOffset+i]/=config->motion.calibration[i];
00304
00305 unsigned int dif=curtime-lastSensorUpdateTime;
00306 lastSensorUpdateTime=curtime;
00307 for(unsigned int i=0; i<evtBuf.size(); i++)
00308 er->postEvent(evtBuf[i]);
00309
00310 er->postEvent(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1);
00311 }
00312
00313
00314 void WorldState::read(const OPowerStatus& power, EventRouter* er) {
00315 std::string actnames[PowerSourceID::NumPowerSIDs];
00316 std::string denames[PowerSourceID::NumPowerSIDs];
00317 unsigned int actmasks[PowerSourceID::NumPowerSIDs];
00318 memset(actmasks,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs);
00319
00320
00321 chkPowerEvent(PowerSourceID::PauseSID, power.robotStatus,orsbPAUSE, "Pause",actnames,denames,actmasks);
00322 chkPowerEvent(PowerSourceID::MotorPowerSID, power.robotStatus,orsbMOTOR_POWER, "MotorPower",actnames,denames,actmasks);
00323 chkPowerEvent(PowerSourceID::VibrationSID, power.robotStatus,orsbVIBRATION_DETECT, "Vibration",actnames,denames,actmasks);
00324 chkPowerEvent(PowerSourceID::ExternalPortSID, power.robotStatus,orsbEX_PORT_CONNECTED, "ExternalPort",actnames,denames,actmasks);
00325 chkPowerEvent(PowerSourceID::StationConnectSID, power.robotStatus,orsbSTATION_CONNECTED, "StationConnect",actnames,denames,actmasks);
00326 chkPowerEvent(PowerSourceID::ExternalPowerSID, power.robotStatus,orsbEX_POWER_CONNECTED, "ExternalPower",actnames,denames,actmasks);
00327 chkPowerEvent(PowerSourceID::BatteryConnectSID, power.robotStatus,orsbBATTERY_CONNECTED, "BatteryConnect",actnames,denames,actmasks);
00328 chkPowerEvent(PowerSourceID::ChargingSID, power.robotStatus,orsbBATTERY_CHARGING, "BatteryCharging",actnames,denames,actmasks);
00329 chkPowerEvent(PowerSourceID::BatteryFullSID, power.robotStatus,orsbBATTERY_CAPACITY_FULL, "BatteryFull",actnames,denames,actmasks);
00330 chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.robotStatus,orsbBATTERY_CAPACITY_LOW, "BatteryLow",actnames,denames,actmasks);
00331 chkPowerEvent(PowerSourceID::OverChargedSID, power.robotStatus,orsbBATTERY_OVER_CURRENT, "BatteryOverCurrent",actnames,denames,actmasks);
00332 chkPowerEvent(PowerSourceID::OverheatingSID, power.robotStatus,orsbBATTERY_OVER_TEMP_DISCHARGING,"BatteryOverTempDischarge",actnames,denames,actmasks);
00333 chkPowerEvent(PowerSourceID::OverheatingSID, power.robotStatus,orsbBATTERY_OVER_TEMP_CHARGING, "BatteryOverTempCharge",actnames,denames,actmasks);
00334 chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbBATTERY_ERROR_OF_CHARGING, "BatteryChargeError",actnames,denames,actmasks);
00335 chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbERROR_OF_PLUNGER, "PlungerError",actnames,denames,actmasks);
00336 chkPowerEvent(PowerSourceID::PowerGoodSID, power.robotStatus,orsbOPEN_R_POWER_GOOD, "PowerGood",actnames,denames,actmasks);
00337 chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbERROR_OF_FAN, "FanError",actnames,denames,actmasks);
00338 chkPowerEvent(PowerSourceID::DataFromStationSID,power.robotStatus,orsbDATA_STREAM_FROM_STATION, "DataFromStation",actnames,denames,actmasks);
00339 chkPowerEvent(PowerSourceID::RegisterUpdateSID, power.robotStatus,orsbREGISTER_UPDATED_BY_STATION, "RegisterUpdate",actnames,denames,actmasks);
00340 chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbRTC_ERROR, "RTCError",actnames,denames,actmasks);
00341 chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_OVERFLOW, "RTCOverflow",actnames,denames,actmasks);
00342 chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_RESET, "RTCReset",actnames,denames,actmasks);
00343 chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_SET, "RTCSet",actnames,denames,actmasks);
00344 chkPowerEvent(PowerSourceID::SpecialModeSID, power.robotStatus,orsbSPECIAL_MODE, "SpecialMode",actnames,denames,actmasks);
00345 chkPowerEvent(PowerSourceID::BMNDebugModeSID, power.robotStatus,orsbBMN_DEBUG_MODE, "BMNDebugMode",actnames,denames,actmasks);
00346 chkPowerEvent(PowerSourceID::ChargerStatusSID, power.robotStatus,orsbCHARGER_STATUS, "ChargerStatus",actnames,denames,actmasks);
00347 chkPowerEvent(PowerSourceID::PlungerSID, power.robotStatus,orsbPLUNGER, "Plunger",actnames,denames,actmasks);
00348 chkPowerEvent(PowerSourceID::SuspendedSID, power.robotStatus,orsbSUSPENDED, "Suspended",actnames,denames,actmasks);
00349
00350
00351 chkPowerEvent(PowerSourceID::ErrorSID, power.batteryStatus,obsbERROR_CODE_MASK, "BatteryError",actnames,denames,actmasks);
00352 chkPowerEvent(PowerSourceID::BatteryEmptySID, power.batteryStatus,obsbFULLY_DISCHARGED, "FullyDischarged",actnames,denames,actmasks);
00353 chkPowerEvent(PowerSourceID::BatteryFullSID, power.batteryStatus,obsbFULLY_CHARGED, "FullyCharged",actnames,denames,actmasks);
00354 chkPowerEvent(PowerSourceID::DischargingSID, power.batteryStatus,obsbDISCHARGING, "Discharging",actnames,denames,actmasks);
00355 chkPowerEvent(PowerSourceID::BatteryInitSID, power.batteryStatus,obsbINITIALIZED, "BatteryInit",actnames,denames,actmasks);
00356 chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_TIME_ALARM, "RemainingTimeAlarm",actnames,denames,actmasks);
00357 chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_CAPACITY_ALARM, "RemainingCapacityAlarm",actnames,denames,actmasks);
00358 chkPowerEvent(PowerSourceID::TermDischargeSID,power.batteryStatus,obsbTERMINATED_DISCHARGING_ALARM,"TermDischargeAlarm",actnames,denames,actmasks);
00359 chkPowerEvent(PowerSourceID::OverheatingSID, power.batteryStatus,obsbOVER_TEMP_ALARM, "OverTempAlarm",actnames,denames,actmasks);
00360 chkPowerEvent(PowerSourceID::TermChargeSID, power.batteryStatus,obsbTERMINATED_CHARGING_ALARM, "TermChargeAlarm",actnames,denames,actmasks);
00361 chkPowerEvent(PowerSourceID::OverChargedSID, power.batteryStatus,obsbOVER_CHARGED_ALARM, "OverChargeAlarm",actnames,denames,actmasks);
00362
00363 sensors[PowerRemainOffset] = power.remainingCapacity/100.0;
00364 sensors[PowerThermoOffset] = power.temperature/100.0;
00365 sensors[PowerCapacityOffset] = power.fullyChargedCapacity;
00366 sensors[PowerVoltageOffset] = power.voltage/1000.0;
00367 sensors[PowerCurrentOffset] = power.current;
00368
00369
00370 for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) {
00371 if(actmasks[i]) {
00372 if(!powerFlags[i])
00373 er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00374 else if(actmasks[i]!=powerFlags[i])
00375 er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00376 } else {
00377 if(powerFlags[i])
00378 er->postEvent(EventBase::powerEGID,i,EventBase::deactivateETID,0,denames[i],0);
00379 }
00380 powerFlags[i]=actmasks[i];
00381 }
00382
00383 er->postEvent(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID,0,"PowerSourceID::UpdatedSID",1);
00384 }
00385
00386 #endif //PLATFORM_APERIOS
00387
00388 void WorldState::chkEvent(std::vector<EventBase*>& evtBuf, unsigned int sid, float newval, const char* name) {
00389 if(newval>=0.1) {
00390 if(buttons[sid]<0.1) {
00391 evtBuf.push_back(new EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,name,newval));
00392 button_times[sid]=curtime;
00393 } else if(alwaysGenerateStatus || buttons[sid]!=newval) {
00394 unsigned int dur=curtime-button_times[sid];
00395 evtBuf.push_back(new EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,name,newval));
00396 }
00397 } else {
00398 if(buttons[sid]>=0.1) {
00399 unsigned int dur=curtime-button_times[sid];
00400 button_times[sid]=0;
00401 evtBuf.push_back(new EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,name,0));
00402 }
00403 }
00404
00405 buttons[sid]=newval;
00406 }
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417