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