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