Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

WorldState.h Go to the documentation of this file. //-*-c++-*- #ifndef INCLUDED_WorldState_h #define INCLUDED_WorldState_h #ifdef PLATFORM_APERIOS #include "IPC/ProcessID.h" #include <OPENR/core_macro.h> #include <OPENR/ObjcommTypes.h> #include <OPENR/OPENR.h> #include <OPENR/OPENRAPI.h> #include <OPENR/OPENRMessages.h> #else class PostureEngine; #endif #include "Shared/RobotInfo.h" #include "Events/EventBase.h" #include <math.h> #include <vector> class EventRouter; class EventBase; //The following SourceIDs are for events created by WorldState's event generators //! holds source ID types for sensor events; see EventBase, see #SensorSourceID_t namespace SensorSourceID { //! holds source ID types for sensor events /*! May want to add a proximity alarm for IR distance? Probably * should do it from a separate generator to avoid screwing up * behaviors relying on the current setup */ enum SensorSourceID_t { UpdatedSID //!< sends status event as last event after processing a frame }; } //! holds source ID types for power events; see EventBase, see #PowerSourceID_t namespace PowerSourceID { //! holds source ID types for power events /*! Also serve as offsets into WorldState::powerFlags[]. * * I've never seen a lot of these events thrown by the OS. 'NS' * means never-seen, which could simply be because i haven't put it * in that situation (don't have a station-type power charger) or * because the OS doesn't actually support sending that flag. * * Under normal conditions, you'll see MotorPowerSID, * BatteryConnectSID, DischargingSID, and PowerGoodSID always * active with occasional VibrationSID and UpdateSID. When the * chest button is pushed, PauseSID is activated and MotorPowerSID * is deactivated. * * The BatteryMonitorBehavior will give a warning once power begins * getting low. The OS won't boot off a battery with less than 15% * power remaining (which is when the LowPowerWarnSID is thrown) * * @note there's not a one-to-one correspondance of the events from * the OPENR power system... i map several OPENR events to fewer * Tekkotsu events, check the event's name if you want to know the * specific source (say if low battery is low current and/or low * voltage) Status ETIDS are only generated when one of a related * group goes on/off but others are still active */ enum PowerSourceID_t { PauseSID=0, //!< the chest button was pushed (this is not a normal button, it kills power to the motors in hardware) MotorPowerSID, //!< active while the motors have power VibrationSID, //!< triggered when the OS decides a large acceleration has occured, like falling down (or more specifically, hitting the ground afterward) BatteryEmptySID, //!< battery is dead LowPowerWarnSID, //!< triggered when sensors[PowerRemainOffset] <= 0.15 (PowerGoodSID stays on) BatteryFullSID, //!< battery is full ExternalPowerSID, //!< receiving power from an external source (such as AC cable, may or may not include the "station", see StationConnectSID) ExternalPortSID, //!< an external power source is plugged in (does not imply current is flowing however) BatteryConnectSID, //!< a battery is plugged in BatteryInitSID, //!< ? NS DischargingSID, //!< using power from the battery (although still stays on after hooked up to external power) 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. OverheatingSID, //!< in case the robot starts getting too hot NS PowerGoodSID, //!< there is power, either from external or battery ChargerStatusSID, //!< ? NS SuspendedSID, //!< ? NS OverChargedSID, //!< in case the charger screws up somehow (?) NS TermDischargeSID, //!< end of battery (?) NS TermChargeSID, //!< end of charging (?) NS ErrorSID, //!< general power error NS StationConnectSID, //!< connected to a station NS BatteryOverCurrentSID, //!< similar to OverChargedSID (?) NS DataFromStationSID, //!< ? NS RegisterUpdateSID, //!< ? NS RTCSID, //!< ? NS SpecialModeSID, //!< ? NS BMNDebugModeSID, //!< ? NS PlungerSID, //!< I think this is in reference to having a memorystick (?) NS UpdatedSID, //!< sent as last event after processing a frame NumPowerSIDs }; } class WorldState; extern WorldState * state; //!< the global state object, is a shared memory region, created by MainObject //! The state of the robot and its environment /*! Contains sensor readings, current joint positions, etc. Notable members are: * - #outputs - joint positions and current LED values * - #buttons - current button values * - #sensors - values from other sensors (IR, acceleration, temperature, power levels) * - #pids - current PID settings for each joint (more specifically, each PID-controlled joint) * - #pidduties - how hard each of the PID joints is working to get to its target value * * Generally you will use enumerated values from a robot-specific namespace to * index into these arrays. Each of those members' documentation specifies where * to find the list of indices to use with them. * * This is a shared memory region between Main, Motion, and possibly others in the future. * Be very careful about including structures that use pointers in * this class... they will only be valid from the OObject that created * them, others may cause a crash if they try to access them. * * WorldState takes power and sensor updates from the system and * maintains the last known values in its member fields. It throws * events when some of these values change, listed in the * SensorSourceID, PowerSourceID namespaces, and the ButtonOffset_t * enumeration for your robot model's info * namespace. (e.g. ERS7Info::ButtonOffset_t) * * Status events for buttons only generated if the * WorldState::alwaysGenerateStatus flag is turned on. Otherwise, by * default, they are only generated when a value has changed. * (i.e. when the pressure sensitive buttons get a new pressure * reading) */ class WorldState { public: //! constructor - sets everything to zeros WorldState(); bool alwaysGenerateStatus; //!< controls whether status events are generated for the boolean buttons float outputs[NumOutputs]; //!< last sensed positions of joints, last commanded value of LEDs; indexes (aka offsets) are defined in the target model's namespace (e.g. "Output Offsets" section of ERS7Info) 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. ERS7Info::ButtonOffset_t) float sensors[NumSensors]; //!< IR, Accel, Thermo, Power stuff; indexes are defined in SensorOffset_t of the target model's namespace (e.g. ERS7Info::SensorOffset_t) float pids[NumPIDJoints][3]; //!< current PID settings (same ordering as the #outputs), not sensed -- updated by MotionManager whenever it sends a PID setting to the system; note this is only valid for PID joints, has NumPIDJoint entries (as opposed to NumOutputs) 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); note this is only valid for PID joints, has NumPIDJoint entries (as opposed to NumOutputs) float vel_x; //!< the current, egocentric rate of forward locomotion, mm/second float vel_y; //!< the current, egocentric rate of sideways (leftward is positive) locomotion, mm/second float vel_a; //!< the current, egocentric rate of rotational (counterclockwise is positive) locomotion, radian/second unsigned int vel_time; //!< the time at which we began moving along the current velocity vector unsigned int robotStatus; //!< bitmask, see OPENR/OPower.h unsigned int batteryStatus; //!< bitmask, see OPENR/OPower.h unsigned int powerFlags[PowerSourceID::NumPowerSIDs]; //!< bitmasks of similarly-grouped items from previous two masks, corresponds to the PowerSourceID::PowerSourceID_t's unsigned int button_times[NumButtons]; //!< value is time of current press, 0 if not down unsigned int lastSensorUpdateTime; //!< primarily so calcDers can determine the time difference between updates, but others might want to know this too... unsigned int frameNumber; //!< the serial number of the currently available frame unsigned int framesProcessed; //!< the number of sensor updates which have been processed static const double g; //!< the gravitational acceleration of objects on earth static const double IROORDist; //!< If IR returns this, we're out of range #ifdef PLATFORM_APERIOS void read(OSensorFrameVectorData& sensor, WorldState* lastState, EventRouter* er); //!< will process a sensor reading as given by OPEN-R void read(const OPowerStatus& power, EventRouter* er); //!< will process a power status update as given by OPEN-R //! returns the current WorldState instance for the running process, needed if your code may be in a shared memory region; otherwise you can directly access ::state /*! Generally you can access ::state directly, but if your code is running as a member of an object in a shared memory region, * this handles the shared object context switching problem on Aperios, and simply returns ::state on non-Aperios. */ static WorldState* getCurrent() { return stateLookupMap==NULL ? NULL : stateLookupMap[ProcessID::getID()]; } // sets the current WorldState instance for the running process (handles the shared object context switching problem on Aperios, simply returns ::state on non-Aperios) // static void setCurrent(WorldState* st) { stateLookupMap[ProcessID::getID()]=st; } //! sets the lookup table (held in a shared region, WorldStatePool::stateMap) used by getCurrent under Aperios static void setWorldStateLookup(WorldState * curMap[ProcessID::NumProcesses]) { stateLookupMap=curMap; } #else void read(const PostureEngine& pose, WorldState* lastState, EventRouter* er); //!< will process the posture's output positions as a new sensor reading //! returns the current WorldState instance for the running process, needed if your code may be in a shared memory region; otherwise you can directly access ::state /*! Generally you can access ::state directly, but if your code is running as a member of an object in a shared memory region, * this handles the shared object context switching problem on Aperios, and simply returns ::state on non-Aperios. */ static WorldState* getCurrent() { return ::state; } #endif //! bitmask corresponding to OPENR::GetRobotDesign() /*! This allows you to efficiently test different combinations, like * any 2x0 model vs. any 7 model or any 3xx model (when/if the * 3xx's are supported). * * Testing this will give more accurate feedback as to whether * features exist than checking RobotInfo values - to achieve dual * booting, RobotInfo may, for instance, tell you there are two * ears, but if you're running on a 220 the value you set them to * will be ignored */ unsigned int robotDesign; static const unsigned int ERS210Mask=1<<0; //!< use this to test for ERS-210 features static const unsigned int ERS220Mask=1<<1; //!< use this to test for ERS-220 features static const unsigned int ERS2xxMask = ERS210Mask | ERS220Mask; //!< use this to test for ERS-2xx features static const unsigned int ERS7Mask=1<<2; //!< use this to test for ERS-7 features protected: unsigned int curtime; //!< set by read(OSensorFrameVectorData& sensor, EventRouter* er) for chkEvent() so each call doesn't have to //! Tests to see if the button status has changed and post events as needed void chkEvent(std::vector<EventBase>& evtBuf, unsigned int off, float newval, const char* name); //! Tests to see if any button statuses have changed and post events as needed, bases comparison between already filled-in WorldStates void chkEvent(std::vector<EventBase>& evtBuf, WorldState* lastState); //! sets the names of the flags that will be generating events /*! note that this function does not actually do the event posting, * unlike chkEvent() */ void chkPowerEvent(unsigned int sid, unsigned int cur, unsigned int mask, const char* name, std::string actname[PowerSourceID::NumPowerSIDs], std::string dename[PowerSourceID::NumPowerSIDs], unsigned int summask[PowerSourceID::NumPowerSIDs]) { if(cur&mask) { actname[sid]+=name; summask[sid]|=mask; } else if(powerFlags[sid]&mask) dename[sid]+=name; } //! given the next value, calculates and stores the next current, the velocity, and the acceleration /*! @param next the new value that's about to be set * @param cur the previous value * @param vel the previous 1st derivative * @param acc the previous 2nd derivative * @param invtimediff @f$1/(curtime-prevtime)@f$ in seconds*/ inline void calcDers(double next, double& cur, double& vel, double& acc, double invtimediff) { double diff=next-cur; cur=next; next=diff*invtimediff;; diff=next-vel; vel=next; acc=diff*invtimediff; } #ifdef PLATFORM_APERIOS static WorldState ** stateLookupMap; //!< array used by getCurrent()/setCurrent() to store the state in use by each process -- should be initialized by setWorldStateLookup() to point into a shared memory region #endif private: WorldState(const WorldState&); //!< this shouldn't be called... WorldState& operator=(const WorldState&); //!< this shouldn't be called... }; /*! @file * @brief Describes WorldState, maintains information about the robot's environment, namely sensors and power status * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.36 $ * $State: Exp $ * $Date: 2006/10/03 22:00:08 $ */ #endif

Tekkotsu v3.0

Generated Wed Oct 4 00:03:47 2006 by Doxygen 1.4.7