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