Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 #ifdef PLATFORM_APERIOS 00019 #include "WorldStateSerializer.h" 00020 #endif 00021 00022 class EventRouter; 00023 00024 //The following SourceIDs are for events created by WorldState's event generators 00025 00026 //! holds source ID types for button events @see EventBase @see ButtonSourceID_t 00027 namespace ButtonSourceID { 00028 //! holds source ID types for button events 00029 /*! Isn't necessarily a straight mapping to ButtonOffset_t's, depends on model (210 doesn't have tail buttons) @see ButtonOffset_t*/ 00030 enum ButtonSourceID_t { 00031 LFrPawSID = LFrPawOffset, 00032 RFrPawSID = RFrPawOffset, 00033 LBkPawSID = LBkPawOffset, 00034 RBkPawSID = RBkPawOffset, 00035 ChinButSID = ChinButOffset, 00036 BackButSID, 00037 HeadFrButSID, 00038 HeadBkButSID, 00039 TailLeftButSID, 00040 TailCenterButSID, 00041 TailRightButSID 00042 }; 00043 } 00044 00045 //! holds source ID types for sensor events @see EventBase @see SensorSourceID_t 00046 namespace SensorSourceID { 00047 //commented out SIDs aren't actually being generated 00048 //! holds source ID types for sensor events 00049 enum SensorSourceID_t { 00050 /* IRDistSID = NumButtons+IRDistOffset 00051 BAccelSID = NumButtons+BAccelOffset, 00052 LAccelSID = NumButtons+LAccelOffset, 00053 DAccelSID = NumButtons+DAccelOffset, 00054 ThermoSID = NumButtons+ThermoOffset, */ 00055 UpdatedSID //!< sends status event as last event after processing a frame 00056 }; 00057 } 00058 00059 //! holds source ID types for power events @see EventBase @see PowerSourceID_t 00060 namespace PowerSourceID { 00061 //! holds source ID types for power events 00062 /*! Also serve as offsets into ::powerFlags[] 00063 * I've never seen a lot of these events thrown by the OS. NS means never-seen, which 00064 * could simply be because i haven't put it in that situation (don't have a station-type power 00065 * charger) or because the OS doesn't actually support sending that flag. 00066 * 00067 * Under normal conditions, you'll see MotorPowerSID, BatteryConnectSID, DischargingSID, and 00068 * PowerGoodSID always active with occasional VibrationSID and UpdateSID. When the chest 00069 * button is pushed, PauseSID is activated and MotorPowerSID is deactivated. 00070 * 00071 * The BatteryMonitorBehavior will give a warning once power begins getting low. The OS won't 00072 * boot off a battery with less than 15% power remaining (which is when the LowPowerWarnSID is 00073 * thrown) 00074 * 00075 * @note there's not a one-to-one correspondance of the events from the 00076 * OPENR power system... i map several OPENR events to fewer Tekkotsu events, check the name 00077 * if you want to know the specific source (say if low battery is low current and/or low voltage) 00078 * Status ETIDS are only generated when one of a related group goes on/off but others are still active*/ 00079 enum PowerSourceID_t { 00080 PauseSID=0, //!< the chest button was pushed (this is not a normal button, it kills power to the motors in hardware) 00081 MotorPowerSID, //!< active while the motors have power 00082 VibrationSID, //!< triggered when the OS decides a large acceleration has occured, like falling down (or more specifically, hitting the ground afterward) 00083 BatteryEmptySID, //!< battery is dead 00084 LowPowerWarnSID, //!< triggered when sensors[PowerRemainOffset] <= 0.15 (PowerGoodSID stays on) 00085 BatteryFullSID, //!< battery is full 00086 ExternalPowerSID, //!< receiving power from an external source (such as AC cable, may or may not include the "station", see StationConnectSID) 00087 ExternalPortSID, //!< an external power source is plugged in (does not imply current is flowing however) 00088 BatteryConnectSID, //!< a battery is plugged in 00089 BatteryInitSID, //!< ? NS 00090 DischargingSID, //!< using power from the battery (although still stays on after hooked up to external power) 00091 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. 00092 OverheatingSID, //!< in case the robot starts getting too hot NS 00093 PowerGoodSID, //!< there is power, either from external or battery 00094 ChargerStatusSID, //!< ? NS 00095 SuspendedSID, //!< ? NS 00096 OverChargedSID, //!< in case the charger screws up somehow (?) NS 00097 TermDischargeSID, //!< end of battery (?) NS 00098 TermChargeSID, //!< end of charging (?) NS 00099 ErrorSID, //!< general power error NS 00100 StationConnectSID, //!< connected to a station NS 00101 BatteryOverCurrentSID, //!< similar to OverChargedSID (?) NS 00102 DataFromStationSID, //!< ? NS 00103 RegisterUpdateSID, //!< ? NS 00104 RTCSID, //!< ? NS 00105 SpecialModeSID, //!< ? NS 00106 BMNDebugModeSID, //!< ? NS 00107 PlungerSID, //!< I think this is in reference to having a memorystick (?) NS 00108 UpdatedSID, //!<sent as last event after processing a frame 00109 NumPowerSIDs 00110 }; 00111 } 00112 00113 //! The state of the robot and its environment\n 00114 /* Contains sensor readings, current joint positions, etc.\n 00115 * This is a shared memory region between MainObj, MotoObj, and possibly others in the future\n 00116 * Be very careful about including structures that use pointers in this class... they will only be valid 00117 * from the OObject that created them, others may cause a crash if they try to access them. 00118 * 00119 * WorldState takes power and sensor updates from the system and maintains the last known 00120 * values in its member fields. It throws events when some of these values change, listed in 00121 * the ButtonSourceID, SensorSourceID, and PowerSourceID namespaces. 00122 * 00123 * Status events for buttons are always generated unless the WorldState::alwaysGenerateStatus flag 00124 * is turned off, in which case they are only generated when a value has changed (the top head buttons) 00125 */ 00126 class WorldState { 00127 public: 00128 //! constructor - sets everything to zeros 00129 WorldState(); 00130 00131 bool alwaysGenerateStatus; //!< controls whether status events are generated for the boolean buttons 00132 00133 float outputs[NumOutputs]; //!< same format as Motion stuff, for ears, x<.5 is up, x>=.5 is down 00134 float buttons[NumButtons]; //!< magnitude is pressure for some, 0/1 for others 00135 float sensors[NumSensors]; //!< IR, Accel, Thermo, Power stuff 00136 float pids[NumPIDJoints][3]; //!< current PID settings 00137 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. 00138 00139 unsigned int robotStatus; //!< bitmask, see OPENR/OPower.h 00140 unsigned int batteryStatus; //!< bitmask, see OPENR/OPower.h 00141 unsigned int powerFlags[PowerSourceID::NumPowerSIDs]; //!< bitmasks of similarly-grouped items from previous two masks, corresponds to the PowerSourceID_t's 00142 00143 unsigned int button_times[NumButtons]; //!< value is time of current press, 0 if not down 00144 00145 unsigned int lastSensorUpdateTime; //!<primarily so calcDers can determine the time difference between updates, but others might want to know this too... 00146 00147 static const double g; //!< the gravitational acceleration of objects on earth 00148 static const double IROORDist; //!< If IR returns this, we're out of range 00149 00150 ProfilerOfSize<20> mainProfile; //!< holds code profiling information for MainObject 00151 ProfilerOfSize<06> motionProfile; //!< holds code profiling information for MotionObject 00152 00153 #ifdef PLATFORM_APERIOS 00154 void read(OSensorFrameVectorData& sensor, EventRouter* er); //!< will process a sensor reading as given by OPEN-R 00155 void read(const OPowerStatus& power, EventRouter* er); //!< will process a power status update as given by OPEN-R 00156 00157 WorldStateSerializer * wsser; //!< sends current worldstate to TekkotsuMon, if it's connected 00158 #endif 00159 00160 //! bitmask corresponding to OPENR::GetRobotDesign() 00161 /*! This allows you to efficiently test different combinations, like any 00162 * 2x0 model vs. any 3xx model (when/if the 3xx's are supported).\n 00163 * Testing this will give more accurate feedback as to whether features 00164 * exist than checking RobotInfo values - to achieve dual booting, RobotInfo 00165 * may, for instance, tell you there are two ears, but if you're running on a 00166 * 220 the value you set them to will be ignored */ 00167 unsigned int robotDesign; 00168 static const unsigned int ERS210Mask=1<<0; //!< use this to test for ERS210 features 00169 static const unsigned int ERS220Mask=1<<1; //!< use this to test for ERS220 features 00170 00171 protected: 00172 unsigned int curtime; //!< set by read(OSensorFrameVectorData& sensor, EventRouter* er) for chkEvent() so each call doesn't have to 00173 00174 //! Tests to see if the button status has changed and post events as needed 00175 void chkEvent(EventRouter* er, unsigned int off, float newval, const char* name); 00176 00177 //! sets the names of the flags that will be generating events 00178 /*! note that this function does not actually do the event posting */ 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-1_4_1 $ 00218 * $Revision: 1.18 $ 00219 * $State: Exp $ 00220 * $Date: 2003/06/12 23:41:41 $ 00221 */ 00222 00223 #endif
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2 |