Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
WorldState.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_WorldState_h 00003 #define INCLUDED_WorldState_h 00004 00005 #ifdef PLATFORM_APERIOS 00006 #include <OPENR/core_macro.h> 00007 #include <OPENR/ObjcommTypes.h> 00008 #include <OPENR/OPENR.h> 00009 #include <OPENR/OPENRAPI.h> 00010 #include <OPENR/OPENRMessages.h> 00011 #endif 00012 00013 #include "Shared/RobotInfo.h" 00014 #include "Events/EventBase.h" 00015 #include "Shared/Profiler.h" 00016 #include <math.h> 00017 00018 class EventRouter; 00019 00020 //The following SourceIDs are for events created by WorldState's event generators 00021 00022 //! holds source ID types for sensor events; see EventBase, see #SensorSourceID_t 00023 namespace SensorSourceID { 00024 //! holds source ID types for sensor events 00025 /*! May want to add a proximity alarm for IR distance? Probably 00026 * should do it from a separate generator to avoid screwing up 00027 * behaviors relying on the current setup 00028 */ 00029 enum SensorSourceID_t { 00030 UpdatedSID //!< sends status event as last event after processing a frame 00031 }; 00032 } 00033 00034 //! holds source ID types for power events; see EventBase, see #PowerSourceID_t 00035 namespace PowerSourceID { 00036 //! holds source ID types for power events 00037 /*! Also serve as offsets into WorldState::powerFlags[]. 00038 * 00039 * I've never seen a lot of these events thrown by the OS. 'NS' 00040 * means never-seen, which could simply be because i haven't put it 00041 * in that situation (don't have a station-type power charger) or 00042 * because the OS doesn't actually support sending that flag. 00043 * 00044 * Under normal conditions, you'll see MotorPowerSID, 00045 * BatteryConnectSID, DischargingSID, and PowerGoodSID always 00046 * active with occasional VibrationSID and UpdateSID. When the 00047 * chest button is pushed, PauseSID is activated and MotorPowerSID 00048 * is deactivated. 00049 * 00050 * The BatteryMonitorBehavior will give a warning once power begins 00051 * getting low. The OS won't boot off a battery with less than 15% 00052 * power remaining (which is when the LowPowerWarnSID is thrown) 00053 * 00054 * @note there's not a one-to-one correspondance of the events from 00055 * the OPENR power system... i map several OPENR events to fewer 00056 * Tekkotsu events, check the event's name if you want to know the 00057 * specific source (say if low battery is low current and/or low 00058 * voltage) Status ETIDS are only generated when one of a related 00059 * group goes on/off but others are still active 00060 */ 00061 enum PowerSourceID_t { 00062 PauseSID=0, //!< the chest button was pushed (this is not a normal button, it kills power to the motors in hardware) 00063 MotorPowerSID, //!< active while the motors have power 00064 VibrationSID, //!< triggered when the OS decides a large acceleration has occured, like falling down (or more specifically, hitting the ground afterward) 00065 BatteryEmptySID, //!< battery is dead 00066 LowPowerWarnSID, //!< triggered when sensors[PowerRemainOffset] <= 0.15 (PowerGoodSID stays on) 00067 BatteryFullSID, //!< battery is full 00068 ExternalPowerSID, //!< receiving power from an external source (such as AC cable, may or may not include the "station", see StationConnectSID) 00069 ExternalPortSID, //!< an external power source is plugged in (does not imply current is flowing however) 00070 BatteryConnectSID, //!< a battery is plugged in 00071 BatteryInitSID, //!< ? NS 00072 DischargingSID, //!< using power from the battery (although still stays on after hooked up to external power) 00073 ChargingSID, //!< you used to be able to charge while running, tho that has changed in more recent versions of OPEN-R. In any case, I never saw this even when it did work. 00074 OverheatingSID, //!< in case the robot starts getting too hot NS 00075 PowerGoodSID, //!< there is power, either from external or battery 00076 ChargerStatusSID, //!< ? NS 00077 SuspendedSID, //!< ? NS 00078 OverChargedSID, //!< in case the charger screws up somehow (?) NS 00079 TermDischargeSID, //!< end of battery (?) NS 00080 TermChargeSID, //!< end of charging (?) NS 00081 ErrorSID, //!< general power error NS 00082 StationConnectSID, //!< connected to a station NS 00083 BatteryOverCurrentSID, //!< similar to OverChargedSID (?) NS 00084 DataFromStationSID, //!< ? NS 00085 RegisterUpdateSID, //!< ? NS 00086 RTCSID, //!< ? NS 00087 SpecialModeSID, //!< ? NS 00088 BMNDebugModeSID, //!< ? NS 00089 PlungerSID, //!< I think this is in reference to having a memorystick (?) NS 00090 UpdatedSID, //!< sent as last event after processing a frame 00091 NumPowerSIDs 00092 }; 00093 } 00094 00095 //! The state of the robot and its environment 00096 /*! Contains sensor readings, current joint positions, etc.\n 00097 * This is a shared memory region between MainObj, MotoObj, and possibly others in the future 00098 * 00099 * Be very careful about including structures that use pointers in 00100 * this class... they will only be valid from the OObject that created 00101 * them, others may cause a crash if they try to access them. 00102 * 00103 * WorldState takes power and sensor updates from the system and 00104 * maintains the last known values in its member fields. It throws 00105 * events when some of these values change, listed in the 00106 * ButtonSourceID, SensorSourceID, and PowerSourceID namespaces. 00107 * 00108 * Status events for buttons only generated if the 00109 * WorldState::alwaysGenerateStatus flag is turned on, otherwise, by 00110 * default, they are only generated when a value has changed 00111 * (i.e. when the pressure sensitive buttons get a new pressure 00112 * reading) 00113 */ 00114 class WorldState { 00115 public: 00116 //! constructor - sets everything to zeros 00117 WorldState(); 00118 00119 bool alwaysGenerateStatus; //!< controls whether status events are generated for the boolean buttons 00120 00121 float outputs[NumOutputs]; //!< last sensed positions of joints, for ears (or other future "boolean joints"), x<.5 is up, x>=.5 is down; indexes (aka offsets) are defined in the target model's namespace (e.g. ERS210Info) 00122 float buttons[NumButtons]; //!< magnitude is pressure for some, 0/1 for others; indexes are defined in the ButtonOffset_t of the target model's namespace (e.g. ERS210Info::ButtonOffset_t) 00123 float sensors[NumSensors]; //!< IR, Accel, Thermo, Power stuff; indexes are defined in SensorOffset_t of the target model's namespace (e.g. ERS210Info::SensorOffset_t) 00124 float pids[NumPIDJoints][3]; //!< current PID settings (same ordering as the #outputs) 00125 float pidduties[NumPIDJoints]; //!< duty cycles - -1 means the motor is trying to move full power in negative direction, 1 means full power in positive direction, in practice, these values stay rather small - 0.15 is significant force. (same ordering as the #outputs) 00126 00127 float vel_x; //!< the current, egocentric rate of forward locomotion 00128 float vel_y; //!< the current, egocentric rate of sideways (leftward is positive) locomotion 00129 float vel_a; //!< the current, egocentric rate of rotational (counterclockwise is positive) locomotion 00130 unsigned int vel_time; //!< the time at which we began moving along the current velocity vector 00131 00132 unsigned int robotStatus; //!< bitmask, see OPENR/OPower.h 00133 unsigned int batteryStatus; //!< bitmask, see OPENR/OPower.h 00134 unsigned int powerFlags[PowerSourceID::NumPowerSIDs]; //!< bitmasks of similarly-grouped items from previous two masks, corresponds to the PowerSourceID::PowerSourceID_t's 00135 00136 unsigned int button_times[NumButtons]; //!< value is time of current press, 0 if not down 00137 00138 unsigned int lastSensorUpdateTime; //!<primarily so calcDers can determine the time difference between updates, but others might want to know this too... 00139 00140 static const double g; //!< the gravitational acceleration of objects on earth 00141 static const double IROORDist; //!< If IR returns this, we're out of range 00142 00143 ProfilerOfSize<20> mainProfile; //!< holds code profiling information for MainObject 00144 ProfilerOfSize<06> motionProfile; //!< holds code profiling information for MotionObject 00145 00146 #ifdef PLATFORM_APERIOS 00147 void read(OSensorFrameVectorData& sensor, EventRouter* er); //!< will process a sensor reading as given by OPEN-R 00148 void read(const OPowerStatus& power, EventRouter* er); //!< will process a power status update as given by OPEN-R 00149 #endif 00150 00151 //! bitmask corresponding to OPENR::GetRobotDesign() 00152 /*! This allows you to efficiently test different combinations, like 00153 * any 2x0 model vs. any 7 model or any 3xx model (when/if the 00154 * 3xx's are supported). 00155 * 00156 * Testing this will give more accurate feedback as to whether 00157 * features exist than checking RobotInfo values - to achieve dual 00158 * booting, RobotInfo may, for instance, tell you there are two 00159 * ears, but if you're running on a 220 the value you set them to 00160 * will be ignored */ 00161 unsigned int robotDesign; 00162 static const unsigned int ERS210Mask=1<<0; //!< use this to test for ERS-210 features 00163 static const unsigned int ERS220Mask=1<<1; //!< use this to test for ERS-220 features 00164 static const unsigned int ERS7Mask=1<<2; //!< use this to test for ERS-7 features 00165 00166 protected: 00167 unsigned int curtime; //!< set by read(OSensorFrameVectorData& sensor, EventRouter* er) for chkEvent() so each call doesn't have to 00168 00169 //! Tests to see if the button status has changed and post events as needed 00170 void chkEvent(EventRouter* er, unsigned int off, float newval, const char* name); 00171 00172 //! sets the names of the flags that will be generating events 00173 /*! note that this function does not actually do the event posting, 00174 * unlike chkEvent() */ 00175 void chkPowerEvent(unsigned int sid, unsigned int cur, unsigned int mask, const char* name, 00176 std::string actname[PowerSourceID::NumPowerSIDs], 00177 std::string dename[PowerSourceID::NumPowerSIDs], 00178 unsigned int summask[PowerSourceID::NumPowerSIDs]) { 00179 if(cur&mask) { 00180 actname[sid]+=name; 00181 summask[sid]|=mask; 00182 } else if(powerFlags[sid]&mask) 00183 dename[sid]+=name; 00184 } 00185 00186 //! given the next value, calculates and stores the next current, the velocity, and the acceleration 00187 /*! @param next the new value that's about to be set 00188 * @param cur the previous value 00189 * @param vel the previous 1st derivative 00190 * @param acc the previous 2nd derivative 00191 * @param invtimediff @f$1/(curtime-prevtime)@f$ in seconds*/ 00192 inline void calcDers(double next, double& cur, double& vel, double& acc, double invtimediff) { 00193 double diff=next-cur; 00194 cur=next; 00195 next=diff*invtimediff;; 00196 diff=next-vel; 00197 vel=next; 00198 acc=diff*invtimediff; 00199 } 00200 00201 private: 00202 WorldState(const WorldState&); //!< don't use 00203 WorldState operator=(const WorldState&); //!< don't use 00204 }; 00205 00206 extern WorldState * state; //!< the global state object, is a shared memory region, created by MainObject 00207 00208 /*! @file 00209 * @brief Describes WorldState, maintains information about the robot's environment, namely sensors and power status 00210 * @author ejt (Creator) 00211 * 00212 * $Author: ejt $ 00213 * $Name: tekkotsu-2_1 $ 00214 * $Revision: 1.25 $ 00215 * $State: Exp $ 00216 * $Date: 2004/02/09 22:45:28 $ 00217 */ 00218 00219 #endif |
Tekkotsu v2.1 |
Generated Tue Mar 16 23:19:16 2004 by Doxygen 1.3.5 |