Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

WorldState.cc

Go to the documentation of this file.
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 
00016 #define GETD(cpc) (((float)sensor.GetData(cpc)->frame[0].value) / 1.0E6f) //!<@return value from OPEN-R, converted from micro in int to base in float
00017 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[0].value) //!<@return value from OPEN-R, as bool
00018 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[0].signal) //!<@return signal from OPEN-R as word
00019 #define GETDUTY(cpc) ((float)((OJointValue*)&sensor.GetData(cpc)->frame[0])->pwmDuty/512.0f) //!<@return duty cycle from OPEN-R as float; -1 (full reverse) to 0 (idle) to 1 (full forward)
00020 
00021 const double WorldState::g=9.80665;
00022 const double WorldState::IROORDist = 900.0;
00023 
00024 WorldState * state=NULL;
00025 
00026 WorldState::WorldState()
00027   : alwaysGenerateStatus(false), robotStatus(0), batteryStatus(0),
00028     lastSensorUpdateTime(0), mainProfile(), motionProfile(),
00029 #ifdef PLATFORM_APERIOS
00030     wsser(NULL), 
00031 #endif
00032     robotDesign(0), curtime(get_time())
00033 {
00034   for(unsigned int i=0; i< NumOutputs; i++)
00035     outputs[i]=0;
00036   for(unsigned int i=0; i< NumButtons; i++)
00037     buttons[i]=0;
00038   for(unsigned int i=0; i< NumSensors; i++)
00039     sensors[i]=0;
00040   for(unsigned int i=0; i< NumPIDJoints; i++)
00041     for(unsigned int j=0; j<3; j++)
00042       pids[i][j]=0;
00043   for(unsigned int i=0; i< NumPIDJoints; i++)
00044     pidduties[i]=0;
00045   memset(powerFlags,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs);
00046   memset(button_times,0,sizeof(unsigned int)*NumButtons);
00047 #ifdef PLATFORM_APERIOS
00048   wsser=new WorldStateSerializer();
00049 #endif
00050   //Thanks Daishi:
00051   char robotDesignStr[orobotdesignNAME_MAX];
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 {
00061       cout << "ERROR: Unrecognized model: "<<robotDesignStr<<"\nTrying ERS-210..."<<endl;
00062       robotDesign=ERS210Mask;
00063     }
00064   }
00065 }
00066 
00067 #ifdef PLATFORM_APERIOS
00068 
00069 /*! This will cause events to be posted
00070  *  @todo change to use most recent instead of oldest - is a buffer! */
00071 void WorldState::read(OSensorFrameVectorData& sensor, EventRouter* er) {
00072   curtime=get_time();
00073 
00074   if(robotDesign&ERS210Mask) {
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     outputs[ERS210Info::TailOffset+TiltOffset] = GETD(ERS210Info::CPCJointTailTilt);
00112     outputs[ERS210Info::TailOffset+PanOffset]  = GETD(ERS210Info::CPCJointTailPan);
00113     pidduties[ERS210Info::TailOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointTailTilt);
00114     pidduties[ERS210Info::TailOffset+PanOffset]  = GETDUTY(ERS210Info::CPCJointTailPan);
00115     
00116     outputs[ERS210Info::MouthOffset] = GETD(ERS210Info::CPCJointMouth);
00117     pidduties[ERS210Info::MouthOffset] = GETDUTY(ERS210Info::CPCJointMouth);
00118 
00119     // Get foot switches
00120     chkEvent(er,LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),"LFrPaw");
00121     chkEvent(er,RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),"RFrPaw");
00122     chkEvent(er,LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),"LBkPaw");
00123     chkEvent(er,RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),"RBkPaw");
00124 
00125     // Get buttons
00126     chkEvent(er,ChinButOffset,  ((OSwitchValue)sensor.GetData(ERS210Info::CPCSensorChinSwitch)->frame[0].value == oswitchON)?1:0,"ChinBut");
00127     chkEvent(er,BackButOffset,  ((OSwitchValue)sensor.GetData(ERS210Info::CPCSensorBackSwitch)->frame[0].value == oswitchON)?1:0,"BackBut");
00128     chkEvent(er,HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),"HeadFrBut");
00129     chkEvent(er,HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),"HeadBkBut");
00130 
00131     // Get IR distance sensor
00132     sensors[IRDistOffset]=((float)sensor.GetData(ERS210Info::CPCSensorPSD)->frame[0].value) / 1000.0;
00133 
00134     // Get acceleration sensors
00135     sensors[BAccelOffset] = GETD(ERS210Info::CPCSensorAccelFB);
00136     sensors[LAccelOffset] = GETD(ERS210Info::CPCSensorAccelLR);
00137     sensors[DAccelOffset] = GETD(ERS210Info::CPCSensorAccelUD);
00138 
00139     sensors[ThermoOffset] = GETD(ERS210Info::CPCSensorThermoSensor);
00140   }
00141 
00142   // (ERS-220 only)
00143   if(robotDesign&ERS220Mask) {
00144     outputs[LFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLFRotator);
00145     outputs[LFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLFElevator);
00146     outputs[LFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLFKnee);
00147     pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLFRotator);
00148     pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLFElevator);
00149     pidduties[LFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLFKnee);
00150   
00151     outputs[RFrLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRFRotator);
00152     outputs[RFrLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRFElevator);
00153     outputs[RFrLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRFKnee);
00154     pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRFRotator);
00155     pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRFElevator);
00156     pidduties[RFrLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRFKnee);
00157   
00158     outputs[LBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointLHRotator);
00159     outputs[LBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointLHElevator);
00160     outputs[LBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointLHKnee);
00161     pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLHRotator);
00162     pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLHElevator);
00163     pidduties[LBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointLHKnee);
00164 
00165     outputs[RBkLegOffset + RotatorOffset   ] = GETD(ERS220Info::CPCJointRHRotator);
00166     outputs[RBkLegOffset + ElevatorOffset  ] = GETD(ERS220Info::CPCJointRHElevator);
00167     outputs[RBkLegOffset + KneeOffset      ] = GETD(ERS220Info::CPCJointRHKnee);
00168     pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRHRotator);
00169     pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRHElevator);
00170     pidduties[RBkLegOffset + KneeOffset    ] = GETDUTY(ERS220Info::CPCJointRHKnee);
00171 
00172     // Get head tilt,pan,roll joint angles
00173     outputs[HeadOffset+TiltOffset] = GETD(ERS220Info::CPCJointNeckTilt);
00174     outputs[HeadOffset+PanOffset ] = GETD(ERS220Info::CPCJointNeckPan);
00175     outputs[HeadOffset+RollOffset] = GETD(ERS220Info::CPCJointNeckRoll);
00176     pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS220Info::CPCJointNeckTilt);
00177     pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS220Info::CPCJointNeckPan);
00178     pidduties[HeadOffset+RollOffset] = GETDUTY(ERS220Info::CPCJointNeckRoll);
00179 
00180     chkEvent(er, ERS220Info::TailLeftButOffset, GETB(ERS220Info::CPCSensorTailLeftSwitch),  "LBkBut");
00181     chkEvent(er, ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),"CBkBut");
00182     chkEvent(er, ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), "RBkBut");
00183 
00184     // Get foot switches
00185     chkEvent(er,LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),"LFrPaw");
00186     chkEvent(er,RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),"RFrPaw");
00187     chkEvent(er,LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),"LBkPaw");
00188     chkEvent(er,RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),"RBkPaw");
00189 
00190     // Get buttons
00191     chkEvent(er,ChinButOffset,  ((OSwitchValue)sensor.GetData(ERS220Info::CPCSensorChinSwitch)->frame[0].value == oswitchON)?1:0,"ChinBut");
00192     chkEvent(er,BackButOffset,  ((OSwitchValue)sensor.GetData(ERS220Info::CPCSensorBackSwitch)->frame[0].value == oswitchON)?1:0,"BackBut");
00193     chkEvent(er,HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),"HeadFrBut");
00194     chkEvent(er,HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),"HeadBkBut");
00195 
00196     // Get IR distance sensor
00197     sensors[IRDistOffset]=((float)sensor.GetData(ERS220Info::CPCSensorPSD)->frame[0].value) / 1000.0;
00198 
00199     // Get acceleration sensors
00200     sensors[BAccelOffset] = GETD(ERS220Info::CPCSensorAccelFB);
00201     sensors[LAccelOffset] = GETD(ERS220Info::CPCSensorAccelLR);
00202     sensors[DAccelOffset] = GETD(ERS220Info::CPCSensorAccelUD);
00203 
00204     sensors[ThermoOffset] = GETD(ERS220Info::CPCSensorThermoSensor);
00205   }
00206 
00207   unsigned int dif=curtime-lastSensorUpdateTime;
00208   lastSensorUpdateTime=curtime;
00209   er->postEvent(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID,dif);
00210 }
00211 
00212 /*! This will cause events to be posted */
00213 void WorldState::read(const OPowerStatus& power, EventRouter* er) {
00214   std::string actnames[PowerSourceID::NumPowerSIDs];
00215   std::string denames[PowerSourceID::NumPowerSIDs];
00216   unsigned int actmasks[PowerSourceID::NumPowerSIDs];
00217   memset(actmasks,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs);
00218 
00219   //RobotStatus
00220   chkPowerEvent(PowerSourceID::PauseSID,          power.robotStatus,orsbPAUSE,                        "Pause",actnames,denames,actmasks);
00221   chkPowerEvent(PowerSourceID::MotorPowerSID,     power.robotStatus,orsbMOTOR_POWER,                  "MotorPower",actnames,denames,actmasks);
00222   chkPowerEvent(PowerSourceID::VibrationSID,      power.robotStatus,orsbVIBRATION_DETECT,             "Vibration",actnames,denames,actmasks);
00223   chkPowerEvent(PowerSourceID::ExternalPortSID,   power.robotStatus,orsbEX_PORT_CONNECTED,            "ExternalPort",actnames,denames,actmasks);
00224   chkPowerEvent(PowerSourceID::StationConnectSID, power.robotStatus,orsbSTATION_CONNECTED,            "StationConnect",actnames,denames,actmasks);
00225   chkPowerEvent(PowerSourceID::ExternalPowerSID,  power.robotStatus,orsbEX_POWER_CONNECTED,           "ExternalPower",actnames,denames,actmasks);
00226   chkPowerEvent(PowerSourceID::BatteryConnectSID, power.robotStatus,orsbBATTERY_CONNECTED,            "BatteryConnect",actnames,denames,actmasks);
00227   chkPowerEvent(PowerSourceID::ChargingSID,       power.robotStatus,orsbBATTERY_CHARGING,             "BatteryCharging",actnames,denames,actmasks);
00228   chkPowerEvent(PowerSourceID::BatteryFullSID,    power.robotStatus,orsbBATTERY_CAPACITY_FULL,        "BatteryFull",actnames,denames,actmasks);
00229   chkPowerEvent(PowerSourceID::LowPowerWarnSID,   power.robotStatus,orsbBATTERY_CAPACITY_LOW,         "BatteryLow",actnames,denames,actmasks);
00230   chkPowerEvent(PowerSourceID::OverChargedSID,    power.robotStatus,orsbBATTERY_OVER_CURRENT,         "BatteryOverCurrent",actnames,denames,actmasks);
00231   chkPowerEvent(PowerSourceID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_DISCHARGING,"BatteryOverTempDischarge",actnames,denames,actmasks);
00232   chkPowerEvent(PowerSourceID::OverheatingSID,    power.robotStatus,orsbBATTERY_OVER_TEMP_CHARGING,   "BatteryOverTempCharge",actnames,denames,actmasks);
00233   chkPowerEvent(PowerSourceID::ErrorSID,          power.robotStatus,orsbBATTERY_ERROR_OF_CHARGING,    "BatteryChargeError",actnames,denames,actmasks);
00234   chkPowerEvent(PowerSourceID::ErrorSID,          power.robotStatus,orsbERROR_OF_PLUNGER,             "PlungerError",actnames,denames,actmasks);
00235   chkPowerEvent(PowerSourceID::PowerGoodSID,      power.robotStatus,orsbOPEN_R_POWER_GOOD,            "PowerGood",actnames,denames,actmasks);
00236   chkPowerEvent(PowerSourceID::ErrorSID,          power.robotStatus,orsbERROR_OF_FAN,                 "FanError",actnames,denames,actmasks);
00237   chkPowerEvent(PowerSourceID::DataFromStationSID,power.robotStatus,orsbDATA_STREAM_FROM_STATION,     "DataFromStation",actnames,denames,actmasks);
00238   chkPowerEvent(PowerSourceID::RegisterUpdateSID, power.robotStatus,orsbREGISTER_UPDATED_BY_STATION,  "RegisterUpdate",actnames,denames,actmasks);
00239   chkPowerEvent(PowerSourceID::ErrorSID,          power.robotStatus,orsbRTC_ERROR,                    "RTCError",actnames,denames,actmasks);
00240   chkPowerEvent(PowerSourceID::RTCSID,            power.robotStatus,orsbRTC_OVERFLOW,                 "RTCOverflow",actnames,denames,actmasks);
00241   chkPowerEvent(PowerSourceID::RTCSID,            power.robotStatus,orsbRTC_RESET,                    "RTCReset",actnames,denames,actmasks);
00242   chkPowerEvent(PowerSourceID::RTCSID,            power.robotStatus,orsbRTC_SET,                      "RTCSet",actnames,denames,actmasks);
00243   chkPowerEvent(PowerSourceID::SpecialModeSID,    power.robotStatus,orsbSPECIAL_MODE,                 "SpecialMode",actnames,denames,actmasks);
00244   chkPowerEvent(PowerSourceID::BMNDebugModeSID,   power.robotStatus,orsbBMN_DEBUG_MODE,               "BMNDebugMode",actnames,denames,actmasks);
00245   chkPowerEvent(PowerSourceID::ChargerStatusSID,  power.robotStatus,orsbCHARGER_STATUS,               "ChargerStatus",actnames,denames,actmasks);
00246   chkPowerEvent(PowerSourceID::PlungerSID,        power.robotStatus,orsbPLUNGER,                      "Plunger",actnames,denames,actmasks);
00247   chkPowerEvent(PowerSourceID::SuspendedSID,      power.robotStatus,orsbSUSPENDED,                    "Suspended",actnames,denames,actmasks);
00248 
00249   //BatteryStatus
00250   chkPowerEvent(PowerSourceID::ErrorSID,        power.batteryStatus,obsbERROR_CODE_MASK,             "BatteryError",actnames,denames,actmasks);
00251   chkPowerEvent(PowerSourceID::BatteryEmptySID, power.batteryStatus,obsbFULLY_DISCHARGED,            "FullyDischarged",actnames,denames,actmasks);
00252   chkPowerEvent(PowerSourceID::BatteryFullSID,  power.batteryStatus,obsbFULLY_CHARGED,               "FullyCharged",actnames,denames,actmasks);
00253   chkPowerEvent(PowerSourceID::DischargingSID,  power.batteryStatus,obsbDISCHARGING,                 "Discharging",actnames,denames,actmasks);
00254   chkPowerEvent(PowerSourceID::BatteryInitSID,  power.batteryStatus,obsbINITIALIZED,                 "BatteryInit",actnames,denames,actmasks);
00255   chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_TIME_ALARM,        "RemainingTimeAlarm",actnames,denames,actmasks);
00256   chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_CAPACITY_ALARM,    "RemainingCapacityAlarm",actnames,denames,actmasks);
00257   chkPowerEvent(PowerSourceID::TermDischargeSID,power.batteryStatus,obsbTERMINATED_DISCHARGING_ALARM,"TermDischargeAlarm",actnames,denames,actmasks);
00258   chkPowerEvent(PowerSourceID::OverheatingSID,  power.batteryStatus,obsbOVER_TEMP_ALARM,             "OverTempAlarm",actnames,denames,actmasks);
00259   chkPowerEvent(PowerSourceID::TermChargeSID,   power.batteryStatus,obsbTERMINATED_CHARGING_ALARM,   "TermChargeAlarm",actnames,denames,actmasks);
00260   chkPowerEvent(PowerSourceID::OverChargedSID,  power.batteryStatus,obsbOVER_CHARGED_ALARM,          "OverChargeAlarm",actnames,denames,actmasks);
00261   
00262   sensors[PowerRemainOffset] = power.remainingCapacity/100.0;
00263   sensors[PowerThermoOffset] = power.temperature/100.0;
00264   sensors[PowerCapacityOffset] = power.fullyChargedCapacity;
00265   sensors[PowerVoltageOffset] = power.voltage/1000.0;
00266   sensors[PowerCurrentOffset] = power.current;
00267 
00268   //only generate status events when a change happens
00269   for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) {
00270     if(actmasks[i]) { //now on
00271       if(!powerFlags[i]) //was off: activation
00272         er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00273       else if(actmasks[i]!=powerFlags[i]) //already on - change? : status
00274         er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00275     } else { // now off
00276       if(powerFlags[i]) //was on: deactivation
00277         er->postEvent(EventBase::powerEGID,i,EventBase::deactivateETID,0,denames[i],0);
00278     }
00279     powerFlags[i]=actmasks[i];
00280   }
00281 
00282   er->postEvent(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID,0);
00283 }
00284 
00285 #endif //PLATFORM_APERIOS
00286 
00287 void WorldState::chkEvent(EventRouter* er, unsigned int off, float newval, const char* name) {
00288   ButtonSourceID::ButtonSourceID_t sid=static_cast<ButtonSourceID::ButtonSourceID_t>(off); //should be a direct conversion
00289   if(newval>=0.1) { //now on
00290     if(buttons[sid]<0.1) { //was off: activation
00291       er->postEvent(EventBase::buttonEGID,sid,EventBase::activateETID,0,name,newval);
00292       button_times[sid]=curtime;
00293     } else if(alwaysGenerateStatus || buttons[sid]!=newval) { //already on - always or change? : status
00294       unsigned int dur=curtime-button_times[sid];
00295       er->postEvent(EventBase::buttonEGID,sid,EventBase::statusETID,dur,name,newval);
00296     }
00297   } else { //now off
00298     if(buttons[sid]>=0.1) { //was on: deactivation
00299       unsigned int dur=curtime-button_times[sid];
00300       button_times[sid]=0;
00301       er->postEvent(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,name,0);
00302     }
00303   }
00304   //update value
00305   buttons[sid]=newval;
00306 }
00307 
00308 /*! @file
00309  * @brief Implements WorldState, maintains information about the robot's environment, namely sensors and power status
00310  * @author ejt (Creator)
00311  *
00312  * $Author: ejt $
00313  * $Name: tekkotsu-1_4_1 $
00314  * $Revision: 1.15 $
00315  * $State: Exp $
00316  * $Date: 2003/06/12 04:16:44 $
00317  */

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2