naowalkoptimiser: /Users/jason/Code/naowalkoptimiser/src/Locomotion/Walk/sensors.h Source File

00001 
00013 #ifndef SENSORS_H
00014 #define SENSORS_H
00015 
00016 #include "jwalkincludes.h"
00017 
00018 #define SENSOR_VERBOSITY        0
00019     
00020 #define SENSOR_LOGGING          0               // enable logging of all data by setting this to 1
00021 #define SENSOR_ULTRASONIC_ON    1               // enable the ultrasonic sensors by setting this to 1
00022 #define SENSOR_EXPORT_TO_AL     1               // set this to 1 to export additional soft sensors to almemory
00023 
00024 
00025 /**************************************************************************************
00026 Sensor Device Names
00027 **************************************************************************************/
00028 extern string indexToName[];
00029 
00030 // Position
00031 extern string indexToPositionSensor[];
00032 #define DN_HEAD_YAW_POSITION          std::string("Device/SubDeviceList/HeadYaw/Position/Sensor/Value")
00033 #define DN_HEAD_PITCH_POSITION        std::string("Device/SubDeviceList/HeadPitch/Position/Sensor/Value")
00034 #define DN_L_SHOULDER_ROLL_POSITION   std::string("Device/SubDeviceList/LShoulderRoll/Position/Sensor/Value")
00035 #define DN_L_SHOULDER_PITCH_POSITION  std::string("Device/SubDeviceList/LShoulderPitch/Position/Sensor/Value")
00036 #define DN_L_ELBOW_YAW_POSITION       std::string("Device/SubDeviceList/LElbowYaw/Position/Sensor/Value") 
00037 #define DN_L_ELBOW_ROLL_POSITION      std::string("Device/SubDeviceList/LElbowRoll/Position/Sensor/Value")
00038 #define DN_R_SHOULDER_ROLL_POSITION   std::string("Device/SubDeviceList/RShoulderRoll/Position/Sensor/Value")
00039 #define DN_R_SHOULDER_PITCH_POSITION  std::string("Device/SubDeviceList/RShoulderPitch/Position/Sensor/Value")
00040 #define DN_R_ELBOW_YAW_POSITION       std::string("Device/SubDeviceList/RElbowYaw/Position/Sensor/Value") 
00041 #define DN_R_ELBOW_ROLL_POSITION      std::string("Device/SubDeviceList/RElbowRoll/Position/Sensor/Value")
00042 #define DN_L_HIP_YAWPITCH_POSITION    std::string("Device/SubDeviceList/LHipYawPitch/Position/Sensor/Value") 
00043 #define DN_L_HIP_ROLL_POSITION        std::string("Device/SubDeviceList/LHipRoll/Position/Sensor/Value") 
00044 #define DN_L_HIP_PITCH_POSITION       std::string("Device/SubDeviceList/LHipPitch/Position/Sensor/Value")
00045 #define DN_L_KNEE_PITCH_POSITION      std::string("Device/SubDeviceList/LKneePitch/Position/Sensor/Value")
00046 #define DN_L_ANKLE_PITCH_POSITION     std::string("Device/SubDeviceList/LAnklePitch/Position/Sensor/Value") 
00047 #define DN_L_ANKLE_ROLL_POSITION      std::string("Device/SubDeviceList/LAnkleRoll/Position/Sensor/Value")
00048 #define DN_R_HIP_YAWPITCH_POSITION    std::string("Device/SubDeviceList/LHipYawPitch/Position/Sensor/Value")
00049 #define DN_R_HIP_ROLL_POSITION        std::string("Device/SubDeviceList/RHipRoll/Position/Sensor/Value") 
00050 #define DN_R_HIP_PITCH_POSITION       std::string("Device/SubDeviceList/RHipPitch/Position/Sensor/Value")
00051 #define DN_R_KNEE_PITCH_POSITION      std::string("Device/SubDeviceList/RKneePitch/Position/Sensor/Value")
00052 #define DN_R_ANKLE_PITCH_POSITION     std::string("Device/SubDeviceList/RAnklePitch/Position/Sensor/Value") 
00053 #define DN_R_ANKLE_ROLL_POSITION      std::string("Device/SubDeviceList/RAnkleRoll/Position/Sensor/Value")
00054 
00055 // Current
00056 extern string indexToCurrentSensor[];
00057 #define DN_HEAD_YAW_CURRENT          std::string("Device/SubDeviceList/HeadYaw/ElectricCurrent/Sensor/Value")
00058 #define DN_HEAD_PITCH_CURRENT        std::string("Device/SubDeviceList/HeadPitch/ElectricCurrent/Sensor/Value")
00059 #define DN_L_SHOULDER_ROLL_CURRENT   std::string("Device/SubDeviceList/LShoulderRoll/ElectricCurrent/Sensor/Value")
00060 #define DN_L_SHOULDER_PITCH_CURRENT  std::string("Device/SubDeviceList/LShoulderPitch/ElectricCurrent/Sensor/Value")
00061 #define DN_L_ELBOW_YAW_CURRENT       std::string("Device/SubDeviceList/LElbowYaw/ElectricCurrent/Sensor/Value") 
00062 #define DN_L_ELBOW_ROLL_CURRENT      std::string("Device/SubDeviceList/LElbowRoll/ElectricCurrent/Sensor/Value")
00063 #define DN_R_SHOULDER_ROLL_CURRENT   std::string("Device/SubDeviceList/RShoulderRoll/ElectricCurrent/Sensor/Value")
00064 #define DN_R_SHOULDER_PITCH_CURRENT  std::string("Device/SubDeviceList/RShoulderPitch/ElectricCurrent/Sensor/Value")
00065 #define DN_R_ELBOW_YAW_CURRENT       std::string("Device/SubDeviceList/RElbowYaw/ElectricCurrent/Sensor/Value") 
00066 #define DN_R_ELBOW_ROLL_CURRENT      std::string("Device/SubDeviceList/RElbowRoll/ElectricCurrent/Sensor/Value")
00067 #define DN_L_HIP_YAWPITCH_CURRENT    std::string("Device/SubDeviceList/LHipYawPitch/ElectricCurrent/Sensor/Value") 
00068 #define DN_L_HIP_ROLL_CURRENT        std::string("Device/SubDeviceList/LHipRoll/ElectricCurrent/Sensor/Value") 
00069 #define DN_L_HIP_PITCH_CURRENT       std::string("Device/SubDeviceList/LHipPitch/ElectricCurrent/Sensor/Value")
00070 #define DN_L_KNEE_PITCH_CURRENT      std::string("Device/SubDeviceList/LKneePitch/ElectricCurrent/Sensor/Value")
00071 #define DN_L_ANKLE_PITCH_CURRENT     std::string("Device/SubDeviceList/LAnklePitch/ElectricCurrent/Sensor/Value") 
00072 #define DN_L_ANKLE_ROLL_CURRENT      std::string("Device/SubDeviceList/LAnkleRoll/ElectricCurrent/Sensor/Value")
00073 #define DN_R_HIP_YAWPITCH_CURRENT    std::string("Device/SubDeviceList/LHipYawPitch/ElectricCurrent/Sensor/Value")
00074 #define DN_R_HIP_ROLL_CURRENT        std::string("Device/SubDeviceList/RHipRoll/ElectricCurrent/Sensor/Value") 
00075 #define DN_R_HIP_PITCH_CURRENT       std::string("Device/SubDeviceList/RHipPitch/ElectricCurrent/Sensor/Value")
00076 #define DN_R_KNEE_PITCH_CURRENT      std::string("Device/SubDeviceList/RKneePitch/ElectricCurrent/Sensor/Value")
00077 #define DN_R_ANKLE_PITCH_CURRENT     std::string("Device/SubDeviceList/RAnklePitch/ElectricCurrent/Sensor/Value") 
00078 #define DN_R_ANKLE_ROLL_CURRENT      std::string("Device/SubDeviceList/RAnkleRoll/ElectricCurrent/Sensor/Value")
00079 
00080 // Actuator Target
00081 extern string indexToTargetSensor[];
00082 #define DN_HEAD_YAW_TARGET          std::string("Device/SubDeviceList/HeadYaw/Position/Actuator/Value")
00083 #define DN_HEAD_PITCH_TARGET        std::string("Device/SubDeviceList/HeadPitch/Position/Actuator/Value")
00084 #define DN_L_SHOULDER_ROLL_TARGET   std::string("Device/SubDeviceList/LShoulderRoll/Position/Actuator/Value")
00085 #define DN_L_SHOULDER_PITCH_TARGET  std::string("Device/SubDeviceList/LShoulderPitch/Position/Actuator/Value")
00086 #define DN_L_ELBOW_YAW_TARGET       std::string("Device/SubDeviceList/LElbowYaw/Position/Actuator/Value") 
00087 #define DN_L_ELBOW_ROLL_TARGET      std::string("Device/SubDeviceList/LElbowRoll/Position/Actuator/Value")
00088 #define DN_R_SHOULDER_ROLL_TARGET   std::string("Device/SubDeviceList/RShoulderRoll/Position/Actuator/Value")
00089 #define DN_R_SHOULDER_PITCH_TARGET  std::string("Device/SubDeviceList/RShoulderPitch/Position/Actuator/Value")
00090 #define DN_R_ELBOW_YAW_TARGET       std::string("Device/SubDeviceList/RElbowYaw/Position/Actuator/Value") 
00091 #define DN_R_ELBOW_ROLL_TARGET      std::string("Device/SubDeviceList/RElbowRoll/Position/Actuator/Value")
00092 #define DN_L_HIP_YAWPITCH_TARGET    std::string("Device/SubDeviceList/LHipYawPitch/Position/Actuator/Value") 
00093 #define DN_L_HIP_ROLL_TARGET        std::string("Device/SubDeviceList/LHipRoll/Position/Actuator/Value") 
00094 #define DN_L_HIP_PITCH_TARGET       std::string("Device/SubDeviceList/LHipPitch/Position/Actuator/Value")
00095 #define DN_L_KNEE_PITCH_TARGET      std::string("Device/SubDeviceList/LKneePitch/Position/Actuator/Value")
00096 #define DN_L_ANKLE_PITCH_TARGET     std::string("Device/SubDeviceList/LAnklePitch/Position/Actuator/Value") 
00097 #define DN_L_ANKLE_ROLL_TARGET      std::string("Device/SubDeviceList/LAnkleRoll/Position/Actuator/Value")
00098 #define DN_R_HIP_YAWPITCH_TARGET    std::string("Device/SubDeviceList/LHipYawPitch/Position/Actuator/Value")
00099 #define DN_R_HIP_ROLL_TARGET        std::string("Device/SubDeviceList/RHipRoll/Position/Actuator/Value") 
00100 #define DN_R_HIP_PITCH_TARGET       std::string("Device/SubDeviceList/RHipPitch/Position/Actuator/Value")
00101 #define DN_R_KNEE_PITCH_TARGET      std::string("Device/SubDeviceList/RKneePitch/Position/Actuator/Value")
00102 #define DN_R_ANKLE_PITCH_TARGET     std::string("Device/SubDeviceList/RAnklePitch/Position/Actuator/Value") 
00103 #define DN_R_ANKLE_ROLL_TARGET      std::string("Device/SubDeviceList/RAnkleRoll/Position/Actuator/Value")
00104 
00105 // Actuator Target
00106 extern string indexToHardnessSensor[];
00107 #define DN_HEAD_YAW_HARDNESS          std::string("Device/SubDeviceList/HeadYaw/Hardness/Actuator/Value")
00108 #define DN_HEAD_PITCH_HARDNESS        std::string("Device/SubDeviceList/HeadPitch/Hardness/Actuator/Value")
00109 #define DN_L_SHOULDER_ROLL_HARDNESS   std::string("Device/SubDeviceList/LShoulderRoll/Hardness/Actuator/Value")
00110 #define DN_L_SHOULDER_PITCH_HARDNESS  std::string("Device/SubDeviceList/LShoulderPitch/Hardness/Actuator/Value")
00111 #define DN_L_ELBOW_YAW_HARDNESS       std::string("Device/SubDeviceList/LElbowYaw/Hardness/Actuator/Value") 
00112 #define DN_L_ELBOW_ROLL_HARDNESS      std::string("Device/SubDeviceList/LElbowRoll/Hardness/Actuator/Value")
00113 #define DN_R_SHOULDER_ROLL_HARDNESS   std::string("Device/SubDeviceList/RShoulderRoll/Hardness/Actuator/Value")
00114 #define DN_R_SHOULDER_PITCH_HARDNESS  std::string("Device/SubDeviceList/RShoulderPitch/Hardness/Actuator/Value")
00115 #define DN_R_ELBOW_YAW_HARDNESS       std::string("Device/SubDeviceList/RElbowYaw/Hardness/Actuator/Value") 
00116 #define DN_R_ELBOW_ROLL_HARDNESS      std::string("Device/SubDeviceList/RElbowRoll/Hardness/Actuator/Value")
00117 #define DN_L_HIP_YAWPITCH_HARDNESS    std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value") 
00118 #define DN_L_HIP_ROLL_HARDNESS        std::string("Device/SubDeviceList/LHipRoll/Hardness/Actuator/Value") 
00119 #define DN_L_HIP_PITCH_HARDNESS       std::string("Device/SubDeviceList/LHipPitch/Hardness/Actuator/Value")
00120 #define DN_L_KNEE_PITCH_HARDNESS      std::string("Device/SubDeviceList/LKneePitch/Hardness/Actuator/Value")
00121 #define DN_L_ANKLE_PITCH_HARDNESS     std::string("Device/SubDeviceList/LAnklePitch/Hardness/Actuator/Value") 
00122 #define DN_L_ANKLE_ROLL_HARDNESS      std::string("Device/SubDeviceList/LAnkleRoll/Hardness/Actuator/Value")
00123 #define DN_R_HIP_YAWPITCH_HARDNESS    std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value")
00124 #define DN_R_HIP_ROLL_HARDNESS        std::string("Device/SubDeviceList/RHipRoll/Hardness/Actuator/Value") 
00125 #define DN_R_HIP_PITCH_HARDNESS       std::string("Device/SubDeviceList/RHipPitch/Hardness/Actuator/Value")
00126 #define DN_R_KNEE_PITCH_HARDNESS      std::string("Device/SubDeviceList/RKneePitch/Hardness/Actuator/Value")
00127 #define DN_R_ANKLE_PITCH_HARDNESS     std::string("Device/SubDeviceList/RAnklePitch/Hardness/Actuator/Value") 
00128 #define DN_R_ANKLE_ROLL_HARDNESS      std::string("Device/SubDeviceList/RAnkleRoll/Hardness/Actuator/Value")
00129 
00130 // Temperature
00131 extern string indexToTemperatureSensor[];
00132 #define DN_HEAD_YAW_TEMPERATURE          std::string("Device/SubDeviceList/HeadYaw/Temperature/Sensor/Value")
00133 #define DN_HEAD_PITCH_TEMPERATURE        std::string("Device/SubDeviceList/HeadPitch/Temperature/Sensor/Value")
00134 #define DN_L_SHOULDER_ROLL_TEMPERATURE   std::string("Device/SubDeviceList/LShoulderRoll/Temperature/Sensor/Value")
00135 #define DN_L_SHOULDER_PITCH_TEMPERATURE  std::string("Device/SubDeviceList/LShoulderPitch/Temperature/Sensor/Value")
00136 #define DN_L_ELBOW_YAW_TEMPERATURE       std::string("Device/SubDeviceList/LElbowYaw/Temperature/Sensor/Value") 
00137 #define DN_L_ELBOW_ROLL_TEMPERATURE      std::string("Device/SubDeviceList/LElbowRoll/Temperature/Sensor/Value")
00138 #define DN_R_SHOULDER_ROLL_TEMPERATURE   std::string("Device/SubDeviceList/RShoulderRoll/Temperature/Sensor/Value")
00139 #define DN_R_SHOULDER_PITCH_TEMPERATURE  std::string("Device/SubDeviceList/RShoulderPitch/Temperature/Sensor/Value")
00140 #define DN_R_ELBOW_YAW_TEMPERATURE       std::string("Device/SubDeviceList/RElbowYaw/Temperature/Sensor/Value") 
00141 #define DN_R_ELBOW_ROLL_TEMPERATURE      std::string("Device/SubDeviceList/RElbowRoll/Temperature/Sensor/Value")
00142 #define DN_L_HIP_YAWPITCH_TEMPERATURE    std::string("Device/SubDeviceList/LHipYawPitch/Temperature/Sensor/Value") 
00143 #define DN_L_HIP_ROLL_TEMPERATURE        std::string("Device/SubDeviceList/LHipRoll/Temperature/Sensor/Value") 
00144 #define DN_L_HIP_PITCH_TEMPERATURE       std::string("Device/SubDeviceList/LHipPitch/Temperature/Sensor/Value")
00145 #define DN_L_KNEE_PITCH_TEMPERATURE      std::string("Device/SubDeviceList/LKneePitch/Temperature/Sensor/Value")
00146 #define DN_L_ANKLE_PITCH_TEMPERATURE     std::string("Device/SubDeviceList/LAnklePitch/Temperature/Sensor/Value") 
00147 #define DN_L_ANKLE_ROLL_TEMPERATURE      std::string("Device/SubDeviceList/LAnkleRoll/Temperature/Sensor/Value")
00148 #define DN_R_HIP_YAWPITCH_TEMPERATURE    std::string("Device/SubDeviceList/LHipYawPitch/Temperature/Sensor/Value")
00149 #define DN_R_HIP_ROLL_TEMPERATURE        std::string("Device/SubDeviceList/RHipRoll/Temperature/Sensor/Value") 
00150 #define DN_R_HIP_PITCH_TEMPERATURE       std::string("Device/SubDeviceList/RHipPitch/Temperature/Sensor/Value")
00151 #define DN_R_KNEE_PITCH_TEMPERATURE      std::string("Device/SubDeviceList/RKneePitch/Temperature/Sensor/Value")
00152 #define DN_R_ANKLE_PITCH_TEMPERATURE     std::string("Device/SubDeviceList/RAnklePitch/Temperature/Sensor/Value") 
00153 #define DN_R_ANKLE_ROLL_TEMPERATURE      std::string("Device/SubDeviceList/RAnkleRoll/Temperature/Sensor/Value")
00154 
00155 // Boards
00156 extern string indexToBoardError[];
00157 #define DN_MB_CHEST                     std::string("Device/DeviceList/ChestBoard/Error")
00158 #define DN_MB_HEAD                      std::string("Device/DeviceList/HeadBoard/Error")
00159 #define DN_MB_R_SHOULDER                std::string("Device/DeviceList/RightShoulderBoard/Error")
00160 #define DN_MB_R_ARM                     std::string("Device/DeviceList/RightArmBoard/Error")
00161 #define DN_MB_R_HAND                    std::string("Device/DeviceList/RightHandBoard/Error")
00162 #define DN_MB_R_HIP                     std::string("Device/DeviceList/RightHipBoard/Error")
00163 #define DN_MB_R_THIGH                   std::string("Device/DeviceList/RightThighBoard/Error")
00164 #define DN_MB_R_SHIN                    std::string("Device/DeviceList/RightShinBoard/Error")
00165 #define DN_MB_R_FOOT                    std::string("Device/DeviceList/RightFootBoard/Error")
00166 #define DN_MB_L_SHOULDER                std::string("Device/DeviceList/LeftShoulderBoard/Error")
00167 #define DN_MB_L_ARM                     std::string("Device/DeviceList/LeftArmBoard/Error")
00168 #define DN_MB_L_HAND                    std::string("Device/DeviceList/LeftHandBoard/Error")
00169 #define DN_MB_L_HIP                     std::string("Device/DeviceList/LeftHipBoard/Error")
00170 #define DN_MB_L_THIGH                   std::string("Device/DeviceList/LeftThighBoard/Error")
00171 #define DN_MB_L_SHIN                    std::string("Device/DeviceList/LeftShinBoard/Error")
00172 #define DN_MB_L_FOOT                    std::string("Device/DeviceList/LeftFootBoard/Error")
00173 #define DN_MB_US                        std::string("Device/DeviceList/USBoard/Error")
00174 #define DN_MB_INERTIAL                  std::string("Device/DeviceList/InertialSensor/Error")
00175 #define DN_MB_TOUCH                     std::string("Device/DeviceList/TouchBoard/Error")
00176 #define DN_MB_FACE                      std::string("Device/DeviceList/FaceBoard/Error")
00177 #define DN_MB_EAR                       std::string("Device/DeviceList/EarLeds/Error")
00178 
00179 // Nacks
00180 extern string indexToNack[];
00181 #define DN_NK_CHEST                     std::string("Device/DeviceList/ChestBoard/Nack")
00182 
00183 // Balance
00184 extern string indexToBalanceSensor[];
00185 #define DN_ACCEL_X                  std::string("Device/SubDeviceList/InertialSensor/AccX/Sensor/Value")
00186 #define DN_ACCEL_Y                  std::string("Device/SubDeviceList/InertialSensor/AccY/Sensor/Value") 
00187 #define DN_ACCEL_Z                  std::string("Device/SubDeviceList/InertialSensor/AccZ/Sensor/Value")
00188 #define DN_ANGLE_X                  std::string("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
00189 #define DN_ANGLE_Y                  std::string("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
00190 #define DN_GYRO_X                   std::string("Device/SubDeviceList/InertialSensor/GyrX/Sensor/Value")
00191 #define DN_GYRO_Y                   std::string("Device/SubDeviceList/InertialSensor/GyrY/Sensor/Value") 
00192 
00193 // Touch
00194 extern string indexToTouchSensor[];
00195 #define DN_L_FSR_FL                 std::string("Device/SubDeviceList/LFoot/FSR/FrontLeft/Sensor/Value")
00196 #define DN_L_FSR_FR                 std::string("Device/SubDeviceList/LFoot/FSR/FrontRight/Sensor/Value")
00197 #define DN_L_FSR_BL                 std::string("Device/SubDeviceList/LFoot/FSR/RearLeft/Sensor/Value")
00198 #define DN_L_FSR_BR                 std::string("Device/SubDeviceList/LFoot/FSR/RearRight/Sensor/Value")
00199 #define DN_L_BUMP_L                 std::string("Device/SubDeviceList/LFoot/Bumper/Left/Sensor/Value")
00200 #define DN_L_BUMP_R                 std::string("Device/SubDeviceList/LFoot/Bumper/Right/Sensor/Value")
00201 #define DN_R_FSR_FL                 std::string("Device/SubDeviceList/RFoot/FSR/FrontLeft/Sensor/Value")
00202 #define DN_R_FSR_FR                 std::string("Device/SubDeviceList/RFoot/FSR/FrontRight/Sensor/Value")
00203 #define DN_R_FSR_BL                 std::string("Device/SubDeviceList/RFoot/FSR/RearLeft/Sensor/Value")
00204 #define DN_R_FSR_BR                 std::string("Device/SubDeviceList/RFoot/FSR/RearRight/Sensor/Value")
00205 #define DN_R_BUMP_L                 std::string("Device/SubDeviceList/RFoot/Bumper/Left/Sensor/Value")
00206 #define DN_R_BUMP_R                 std::string("Device/SubDeviceList/RFoot/Bumper/Right/Sensor/Value")
00207 #define DN_CHEST_BUTTON             std::string("Device/SubDeviceList/ChestBoard/Button/Sensor/Value")
00208 #define DN_SIMPLE_CLICK             std::string("ALWatchDog/SimpleClickOccured")
00209 #define DN_DOUBLE_CLICK             std::string("ALWatchDog/DoubleClickOccured")
00210 #define DN_TRIPLE_CLICK             std::string("ALWatchDog/TripleClickOccured")
00211 
00212 // Battery
00213 extern string indexToBatterySensor[];
00214 #define DN_CHARGE                   std::string("Device/SubDeviceList/Battery/Charge/Sensor/Value")
00215 #define DN_CURRENT                  std::string("Device/SubDeviceList/Battery/Current/Sensor/Value")
00216 #define DN_VOLTAGE_MIN              std::string("Device/SubDeviceList/Battery/Charge/Sensor/CellVoltageMin")
00217 #define DN_VOLTAGE_MAX              std::string("Device/SubDeviceList/Battery/Charge/Sensor/CellVoltageMax")
00218 #define DN_TEMPERATURE              std::string("Device/SubDeviceList/Battery/Temperature/Sensor/Value")
00219 
00220 // Distance (ultrasonic and infrared sensors)
00221 extern string indexToDistanceSensor[];
00222 #define DN_US_DISTANCE              std::string("Device/SubDeviceList/US/Sensor/Value")
00223 //#define DN_L_IR                     std::string("Device/SubDeviceList/LIR/Sensor/Value")
00224 //#define DN_R_IR                     std::string("Device/SubDeviceList/RIR/Sensor/Value")
00225 
00226 /**************************************************************************************
00227 Sensor Array Indices
00228 **************************************************************************************/
00229 
00230 // Joint sensors: Position, Target, Hardness, Current, Temperature indicies
00231 enum{
00232     J_HEAD_YAW,
00233     J_HEAD_PITCH,
00234     J_L_SHOULDER_ROLL,
00235     J_L_SHOULDER_PITCH,
00236     J_L_ELBOW_YAW,
00237     J_L_ELBOW_ROLL,
00238     J_R_SHOULDER_ROLL,
00239     J_R_SHOULDER_PITCH,
00240     J_R_ELBOW_YAW,
00241     J_R_ELBOW_ROLL,
00242     J_L_HIP_YAWPITCH,
00243     J_L_HIP_ROLL,
00244     J_L_HIP_PITCH,
00245     J_L_KNEE_PITCH,
00246     J_L_ANKLE_PITCH,
00247     J_L_ANKLE_ROLL,
00248     J_R_HIP_YAWPITCH,
00249     J_R_HIP_ROLL,
00250     J_R_HIP_PITCH,
00251     J_R_KNEE_PITCH,
00252     J_R_ANKLE_PITCH,
00253     J_R_ANKLE_ROLL,
00254     J_NUM_JOINTS
00255 };
00256 
00257 // Motors Boards:
00258 enum{
00259     MB_CHEST,
00260     MB_HEAD,
00261     MB_R_SHOULDER,
00262     MB_R_ARM,
00263     MB_R_HAND,
00264     MB_R_HIP,
00265     MB_R_THIGH,
00266     MB_R_SHIN,
00267     MB_R_FOOT,
00268     MB_L_SHOULDER,
00269     MB_L_ARM,
00270     MB_L_HAND,
00271     MB_L_HIP,
00272     MB_L_THIGH,
00273     MB_L_SHIN,
00274     MB_L_FOOT,
00275     MB_US,
00276     MB_INERTIAL,
00277     MB_TOUCH,
00278     MB_FACE,
00279     MB_EAR,
00280     MB_NUM_BOARDS
00281 };
00282 
00283 // Nacks
00284 
00285 enum{
00286     NK_CHEST,
00287     NK_NUM_BOARDS
00288 };
00289 
00290 // Balance sensors: accelerometer, gyro and Aldebaran's angle
00291 enum{
00292   // Accelerometers
00293     B_ACCEL_X, 
00294     B_ACCEL_Y, 
00295     B_ACCEL_Z,
00296   // Angles
00297     B_ANGLE_X,
00298     B_ANGLE_Y,
00299   // Gyrometers
00300     B_GYRO_X,
00301     B_GYRO_Y,
00302     B_NUM_SENSORS
00303 };
00304 
00305 // Touch sensors: Feet FSR, feet bumpers, chest button
00306 enum{
00307   // Left Foot Touch Sensors
00308     // Pressure Sensors
00309     T_L_FSR_FL,
00310     T_L_FSR_FR,
00311     T_L_FSR_BL,
00312     T_L_FSR_BR,
00313     // Bumpers
00314     T_L_BUMP_L,
00315     T_L_BUMP_R,
00316   // Right Foot Touch Sensors
00317     // Pressure Sensors
00318     T_R_FSR_FL,
00319     T_R_FSR_FR,
00320     T_R_FSR_BL,
00321     T_R_FSR_BR,
00322     // Bumpers
00323     T_R_BUMP_L,
00324     T_R_BUMP_R,
00325   // Other Touch Sensors
00326     T_CHEST_BUTTON,
00327     T_NUM_SENSORS
00328 };
00329 
00330 // Foot Pressure sensors:
00331 enum{
00332     PS_L_FL,
00333     PS_L_FR,
00334     PS_L_BL,
00335     PS_L_BR,
00336     PS_L,           // the total force (N) on the left foot
00337     PS_R_FL,
00338     PS_R_FR,
00339     PS_R_BL,
00340     PS_R_BR,
00341     PS_R,           // the total force (N) on the right foot
00342     PS_NUM_SENSORS
00343 };
00344 
00345 // Support Mode:
00346 enum SupportModeEnum
00347 {
00348     SM_STANCE,
00349     SM_PUSH,
00350     SM_SWING,
00351     SM_IMPACT,
00352     SM_NUM_MODES
00353 };
00354 extern string indexToSupportMode[SM_NUM_MODES];
00355 
00356 // Energy sensors: battery
00357 enum{
00358     E_CHARGE,
00359     E_CURRENT,
00360     E_VOLTAGE_MIN,
00361     E_VOLTAGE_MAX,
00362     E_TEMPERATURE,
00363     E_NUM_SENSORS
00364 };
00365 
00366 // Distance sensors (just the ultrasonic sensors)
00367 enum{
00368     D_US,
00369     //D_L_IR,
00370     //D_R_IR,
00371     D_NUM_SENSORS
00372 };
00373 
00374 #define ALL_NUM_SENSORS             J_NUM_JOINTS*4 + B_NUM_SENSORS + T_NUM_SENSORS + E_NUM_SENSORS + D_NUM_SENSORS
00375 
00376 // Sonic sensors
00377 enum{
00378     S_LL,
00379     S_LR,
00380     S_RL,
00381     S_RR,
00382     S_NUM_SENSORS
00383 };
00384 
00385 /**************************************************************************************
00386 Sensor Arrays
00387 **************************************************************************************/
00388 
00389 extern int dcmTime;
00390 extern int dcmTimeSinceStart;
00391 
00392 // Proprioception Feedback Data. Velocities and Accelerations are measured by soft sensors
00393 extern float jointPositions[J_NUM_JOINTS];
00394 extern float jointVelocities[J_NUM_JOINTS];
00395 extern float jointVelocitySum;
00396 extern float jointAccelerations[J_NUM_JOINTS];
00397 extern float jointCurrents[J_NUM_JOINTS];
00398 extern float jointCurrentSum;
00399 extern float jointTargets[J_NUM_JOINTS];
00400 extern float jointHardnesses[J_NUM_JOINTS];
00401 extern int boardErrors[MB_NUM_BOARDS];
00402 extern int nackErrors[NK_NUM_BOARDS];
00403 extern unsigned char walkCyclesSinceCall;   // I need this so that walkIsActive is true immediately after a call to walk!
00404 extern bool walkAmIWalking;
00405 extern bool walkPreviousAmIWalking;         // I need this one to detect when the robot stops
00406 
00407 extern float odometryDeltaX;            // x odometry change in cm
00408 extern float odometryDeltaY;            // y odometry change in cm
00409 extern float odometryDeltaO;            // orientation change in cm
00410 
00411 // Thermoception Feedback Data.
00412 extern float jointTemperatures[J_NUM_JOINTS];
00413 
00414 // Balance Feedback Data. Accelerometers and Gyros
00415 extern float balanceValues[B_NUM_SENSORS];
00416 extern bool balanceFallingEnabled;
00417 extern bool balanceFalling;                 // the robot is presently falling
00418 extern bool balancePreviousFalling;
00419 extern bool balanceFallingForward;
00420 extern bool balanceFallingBackward;
00421 extern bool balanceFallingLeft;
00422 extern bool balanceFallingRight;
00423 extern bool balanceFallen;                  // the robot has finished falling, and is now fallen (you can start getting up now)
00424 extern bool balancePreviousFallen;
00425 extern int balanceFallenCount;              // the number of times since boot (or other event) I have fallen over
00426 
00427 // Touch Feedback Data. Foot Pressure sensors, foot bump sensors, and chest buttons
00428 extern float touchValues[T_NUM_SENSORS];
00429 extern float touchLeftCoPX;
00430 extern float touchLeftCoPY;
00431 extern float touchRightCoPX;
00432 extern float touchRightCoPY;
00433 extern float touchPSValues[PS_NUM_SENSORS];
00434 extern bool touchOnGround;
00435 extern bool touchPreviousOnGround;
00436 extern bool touchLeftFootOnGround;
00437 extern bool touchRightFootOnGround;
00438 extern SupportModeEnum leftSupportMode;
00439 extern SupportModeEnum rightSupportMode;
00440 
00441 extern bool collisionAny;
00442 extern bool collisionLeftArm;
00443 extern bool collisionLeftArmFront;
00444 extern bool collisionLeftArmSide;
00445 extern bool collisionLeftArmBack;
00446 extern bool collisionRightArm;
00447 extern bool collisionRightArmFront;
00448 extern bool collisionRightArmSide;
00449 extern bool collisionRightArmBack;
00450 extern bool collisionLeftFoot;
00451 extern bool collisionLeftFootFront;
00452 extern bool collisionLeftFootBack;
00453 extern bool collisionRightFoot;
00454 extern bool collisionRightFootFront;
00455 extern bool collisionRightFootBack;
00456 
00457 // Battery Feedback Data. 
00458 extern float batteryValues[E_NUM_SENSORS];
00459 
00460 // Distance Feedback Data. (centimetres)
00461 //extern float distanceValues[D_NUM_SENSORS];                 // caution: these values are only to be used by me, use the ones below for useful sensor data
00462 extern float sonicValues[S_NUM_SENSORS];
00463 extern float sonicObstacleDistance;
00464 
00465 #define VEL_WINDOW_SIZE     16
00466 #define B_WINDOW_SIZE       9
00467 #define S_WINDOW_SIZE       1
00468 
00469 /**************************************************************************************
00470 Sensor Array Class
00471 **************************************************************************************/
00472 
00473 class Sensors
00474 {
00475   public:
00476     Sensors(AL::ALPtr<AL::ALBroker> pBroker);
00477     virtual ~Sensors();
00478     void onNewSensorData();
00479     void getSensorData();
00480     void writeSensorData();
00481     void createLogs();
00482     void finishLogs();
00483     
00484     void calibrateFootForceSensors();
00485     void calculateOdometry();
00486     
00487     static void sendUSCommandToDCM(float value);
00488     static void sleepMSec(timespec* nextWakeTime, int msec);
00489     
00490   private:
00491     void connectALMemoryFastAccess(AL::ALPtr<AL::ALBroker> pBroker);
00492     
00493     void getTimeData();
00494     void getFastMemData();
00495     
00496     void signCurrentData();
00497     void calculateJointVelocities();
00498     void calculateJointAccelerations();
00499     
00500     void filterData();
00501     void filterBalanceValues();
00502     void filterJointVelocities();
00503     
00504     void calculateSoftSensors();
00505     void calculateStiffnessCorrectionFactor();
00506     void calculateFootForceReadings();
00507     void calculateCoP();
00508     void determineSupportMode();
00509     void determineWhetherOnGround();
00510     void determineWhetherWalking();
00511     void determineWhetherFalling();
00512     void determineWhetherColliding();
00513     
00514     void updateSensorHistory();
00515     
00516     // Data logging functions
00517     void writePositionData();
00518     void writeVelocityData();
00519     void writeAccelerationData();
00520     void writeCurrentData();
00521     void writeTargetData();
00522     void writeHardnessData();
00523     void writeTemperatureData();
00524     void writeBalanceData();
00525     void writeTouchData();
00526     void writeBatteryData();
00527     void writeDistanceData();
00528     
00529     void writeJointLabels(ofstream &logfile);
00530     void writeBalanceLabels();
00531     void writeTouchLabels();
00532     void writeBatteryLabels();
00533     void writeDistanceLabels();
00534 
00535   public:
00536     int historysonicindex[S_NUM_SENSORS];                                          // this has to be public because the data is shared with a thread
00537     float historySonicValues[S_NUM_SENSORS][S_WINDOW_SIZE];
00538     
00539   private:
00540     int dcmStartTime;         // The dcm time when Sensors was created
00541     // Historical sensor data
00542     int previousDcmTime;
00543     float previousJointPositions[J_NUM_JOINTS];
00544     float previousJointVelocities[J_NUM_JOINTS];
00545     float previousJointCurrents[J_NUM_JOINTS];
00546     
00547     int historybalanceindex;
00548     float historyBalanceValues[B_NUM_SENSORS][B_WINDOW_SIZE];
00549     
00550     int historyvelocityindex;
00551     float historyJointVelocities[J_NUM_JOINTS][VEL_WINDOW_SIZE];
00552     
00553     // file streams to write sensor data into
00554 //#if SENSOR_LOGGING
00555     ofstream positionLog;
00556     ofstream velocityLog;
00557     ofstream accelerationLog;
00558     ofstream currentLog;
00559     ofstream targetLog;
00560     ofstream hardnessLog;
00561     ofstream temperatureLog;
00562     ofstream balanceLog;
00563     ofstream touchLog;
00564     ofstream batteryLog;
00565     ofstream distanceLog;
00566 //#endif
00567     
00568     pthread_t SensorUltrasonicThread;
00569 };
00570 
00571 void *runSonicThread(void* arg);            // it is too hard to make a thread function a member of a class
00572 
00573 #endif