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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:50 2005 by Doxygen 1.4.4