Homepage Demos Overview Downloads Tutorials Reference
Credits

WorldState.h

Go 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