Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 #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