Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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 #include "ERS7Info.h"
00016 #include "Shared/Config.h"
00017 
00018 #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
00019 #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value) //!< returns value from OPEN-R, as bool
00020 #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value) //!< return value from OPEN-R, as int
00021 #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal) //!< returns signal from OPEN-R as word
00022 #define GETDUTY(cpc) ((float)((OJointValue*)&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)
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   //Thanks Daishi:
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   //This is only compiled if we're targeting the local platform
00069   //In other words, if target model is an AIBO, simulation mode
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 /*! This will cause events to be posted
00089  *  @todo change to use most recent instead of oldest - is a buffer! */
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     // Get head tilt,pan,roll joint angles
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     // Get foot switches
00142     chkEvent(evtBuf,ERS210Info::LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),buttonNames[ERS210Info::LFrPawOffset]);
00143     chkEvent(evtBuf,ERS210Info::RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),buttonNames[ERS210Info::RFrPawOffset]);
00144     chkEvent(evtBuf,ERS210Info::LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),buttonNames[ERS210Info::LBkPawOffset]);
00145     chkEvent(evtBuf,ERS210Info::RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),buttonNames[ERS210Info::RBkPawOffset]);
00146 
00147     // Get buttons
00148     chkEvent(evtBuf,ERS210Info::ChinButOffset,  GETB(ERS210Info::CPCSensorChinSwitch),buttonNames[ERS210Info::ChinButOffset]);
00149     chkEvent(evtBuf,ERS210Info::BackButOffset,  GETB(ERS210Info::CPCSensorBackSwitch),buttonNames[ERS210Info::BackButOffset]);
00150     chkEvent(evtBuf,ERS210Info::HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),buttonNames[ERS210Info::HeadFrButOffset]);
00151     chkEvent(evtBuf,ERS210Info::HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),buttonNames[ERS210Info::HeadBkButOffset]);
00152 
00153     // Get IR distance sensor
00154     sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f;
00155 
00156     // Get acceleration sensors
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   // (ERS-220 only)
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     // Get head tilt,pan,roll joint angles
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),  buttonNames[ERS220Info::TailLeftButOffset]);
00203     chkEvent(evtBuf, ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),buttonNames[ERS220Info::TailCenterButOffset]);
00204     chkEvent(evtBuf, ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), buttonNames[ERS220Info::TailRightButOffset]);
00205 
00206     // Get foot switches
00207     chkEvent(evtBuf,ERS220Info::LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),buttonNames[ERS220Info::LFrPawOffset]);
00208     chkEvent(evtBuf,ERS220Info::RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),buttonNames[ERS220Info::RFrPawOffset]);
00209     chkEvent(evtBuf,ERS220Info::LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),buttonNames[ERS220Info::LBkPawOffset]);
00210     chkEvent(evtBuf,ERS220Info::RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),buttonNames[ERS220Info::RBkPawOffset]);
00211 
00212     // Get buttons
00213     chkEvent(evtBuf,ERS220Info::ChinButOffset,  GETB(ERS220Info::CPCSensorChinSwitch),buttonNames[ERS220Info::ChinButOffset]);
00214     chkEvent(evtBuf,ERS220Info::BackButOffset,  GETB(ERS220Info::CPCSensorBackSwitch),buttonNames[ERS220Info::BackButOffset]);
00215     chkEvent(evtBuf,ERS220Info::HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),buttonNames[ERS220Info::HeadFrButOffset]);
00216     chkEvent(evtBuf,ERS220Info::HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),buttonNames[ERS220Info::HeadBkButOffset]);
00217 
00218     // Get IR distance sensor
00219     sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f;
00220 
00221     // Get acceleration sensors
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   // (ERS-7 only)
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     // Get head tilt,pan,nod joint angles
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     // Get foot switches
00276     chkEvent(evtBuf,ERS7Info::LFrPawOffset,GETB(ERS7Info::CPCSwitchLFPaw),buttonNames[ERS7Info::LFrPawOffset]);
00277     chkEvent(evtBuf,ERS7Info::RFrPawOffset,GETB(ERS7Info::CPCSwitchRFPaw),buttonNames[ERS7Info::RFrPawOffset]);
00278     chkEvent(evtBuf,ERS7Info::LBkPawOffset,GETB(ERS7Info::CPCSwitchLHPaw),buttonNames[ERS7Info::LBkPawOffset]);
00279     chkEvent(evtBuf,ERS7Info::RBkPawOffset,GETB(ERS7Info::CPCSwitchRHPaw),buttonNames[ERS7Info::RBkPawOffset]);
00280 
00281     // Get buttons/switches
00282     // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210
00283     chkEvent(evtBuf, ERS7Info::ChinButOffset,       GETSENSOR(ERS7Info::CPCSwitchChin),      buttonNames[ERS7Info::ChinButOffset]);
00284     chkEvent(evtBuf, ERS7Info::HeadButOffset,       GETSENSOR(ERS7Info::CPCSensorHead)/120,      buttonNames[ERS7Info::HeadButOffset]);
00285     chkEvent(evtBuf, ERS7Info::FrontBackButOffset,  GETSENSOR(ERS7Info::CPCSensorBackFront)/150, buttonNames[ERS7Info::FrontBackButOffset]);
00286     chkEvent(evtBuf, ERS7Info::MiddleBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150,buttonNames[ERS7Info::MiddleBackButOffset]);
00287     chkEvent(evtBuf, ERS7Info::RearBackButOffset,   GETSENSOR(ERS7Info::CPCSensorBackRear)/150,  buttonNames[ERS7Info::RearBackButOffset]);
00288     chkEvent(evtBuf, ERS7Info::WirelessSwOffset,GETSENSOR(ERS7Info::CPCSwitchWireless),  buttonNames[ERS7Info::WirelessSwOffset]);
00289 
00290     // Get IR distance sensor
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     // Get acceleration sensors
00296     sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB);
00297     sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR);
00298     sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD);
00299   }
00300 
00301   //Apply sensor calibrations (currently only joint positions - perhaps IR as well?)
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   //we don't delete the events in evtBuf - we're handing them off to erouter, which will delete them itself
00310   er->postEvent(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1);
00311 }
00312 
00313 /*! This will cause events to be posted */
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   //RobotStatus
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   //BatteryStatus
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   //only generate status events when a change happens
00370   for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) {
00371     if(actmasks[i]) { //now on
00372       if(!powerFlags[i]) //was off: activation
00373         er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1);
00374       else if(actmasks[i]!=powerFlags[i]) //already on - change? : status
00375         er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1);
00376     } else { // now off
00377       if(powerFlags[i]) //was on: deactivation
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) { //now on
00390     if(buttons[sid]<0.1) { //was off: activation
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) { //already on - always or change? : status
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 { //now off
00398     if(buttons[sid]>=0.1) { //was on: deactivation
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   //update value
00405   buttons[sid]=newval;
00406 }
00407 
00408 /*! @file
00409  * @brief Implements WorldState, maintains information about the robot's environment, namely sensors and power status
00410  * @author ejt (Creator)
00411  *
00412  * $Author: ejt $
00413  * $Name: tekkotsu-2_4_1 $
00414  * $Revision: 1.30 $
00415  * $State: Exp $
00416  * $Date: 2005/07/10 20:02:17 $
00417  */

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:50 2005 by Doxygen 1.4.4