aibosensor.h

Go to the documentation of this file.

// ***************************************************************************
//                           aibonet.h
//
//    copyright            : (C) 2003 by tbaier
//    email                : tbaier@informatik.uni-hamburg.de
// ***************************************************************************

#ifndef AIBOSENSOR_H
#define AIBOSENSOR_H


//std includes
#include <math.h>

//AIBO includes
#include <OPENR/ODataFormats.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <Types.h>

#include "defines.h"

static const int NUM_SENSORS = 31;

enum SENSOR{
  HEAD_TILT,  
  HEAD_PAN,   
  HEAD_ROLL,  
  PSD,        
  TACT_F,     
  TACT_R,     
  TIN_SW,     
  LFLEG_J1,   
  LFLEG_J2,   
  LFLEG_J3,   
  LFLEG_SW,   
  LRLEG_J1,   
  LRLEG_J3,   
  LRLEG_J2,   
  LRLEG_SW,   
  RFLEG_J1,   
  RFLEG_J2,   
  RFLEG_J3,   
  RFLEG_SW,   
  RRLEG_J1,   
  RRLEG_J2,   
  RRLEG_J3,   
  RRLEG_SW,   
  THERMO,     
  BACK_SW,    
  TAIL_SW_L,  
  TAIL_SW_M,  
  TAIL_SW_R,  
  ACC_X,      
  ACC_Y,      
  ACC_Z       
};

enum JOINT_SENSOR{
  J_HEAD_TILT = 0,   
  J_HEAD_PAN  = 1,   
  J_HEAD_ROLL = 2,   
  J_LFLEG_J1  = 7,   
  J_LFLEG_J2  = 8,   
  J_LFLEG_J3  = 9,   
  J_LRLEG_J1  = 11,  
  J_LRLEG_J2  = 12,  
  J_LRLEG_J3  = 13,  
  J_RFLEG_J1  = 15,  
  J_RFLEG_J2  = 16,  
  J_RFLEG_J3  = 17,  
  J_RRLEG_J1  = 19,  
  J_RRLEG_J2  = 20,  
  J_RRLEG_J3  = 21   
};

enum SWITCH{
  S_TACT_F    = 4,     
  S_TACT_R    = 5,     
  S_TIN_SW    = 6,     
  S_LFLEG_SW  = 10,    
  S_LRLEG_SW  = 14,    
  S_RFLEG_SW  = 18,    
  S_RRLEG_SW  = 22 ,   
  S_BACK_SW   = 24,    
  S_TAIL_SW_L = 25,    
  S_TAIL_SW_M = 26,    
  S_TAIL_SW_R = 27     
};


static const char* const SENSOR_ARRAY[] = {
  //Head Joints
  "PRM:/r1/c1-Joint2:j1",             
  "PRM:/r1/c1/c2-Joint2:j2",          
  "PRM:/r1/c1/c2/c3-Joint2:j3",       
  // Head sensors
  "PRM:/r1/c1/c2/c3/p1-Sensor:p1",    
  "PRM:/r1/c1/c2/c3/f1-Sensor:f1",    
  "PRM:/r1/c1/c2/c3/f2-Sensor:f2",    
  "PRM:/r1/c1/c2/c3/c4/s5-Sensor:s5", 
  // LeftFrontLeg
  "PRM:/r2/c1-Joint2:j1",             
  "PRM:/r2/c1/c2-Joint2:j2",          
  "PRM:/r2/c1/c2/c3-Joint2:j3",       
  "PRM:/r2/c1/c2/c3/c4-Sensor:s4",    
  // LeftRearLeg
  "PRM:/r3/c1-Joint2:j1",             
  "PRM:/r3/c1/c2-Joint2:j2",          
  "PRM:/r3/c1/c2/c3-Joint2:j3",       
  "PRM:/r3/c1/c2/c3/c4-Sensor:s4",    
  // RightFrontLeg
  "PRM:/r4/c1-Joint2:j1",             
  "PRM:/r4/c1/c2-Joint2:j2",          
  "PRM:/r4/c1/c2/c3-Joint2:j3",       
  "PRM:/r4/c1/c2/c3/c4-Sensor:s4",    
  // RightRearLeg
  "PRM:/r5/c1-Joint2:j1",              
  "PRM:/r5/c1/c2-Joint2:j2",           
  "PRM:/r5/c1/c2/c3-Joint2:j3",        
  "PRM:/r5/c1/c2/c3/c4-Sensor:s4",     
  // Back sensors
  "PRM:/r6/t1-Sensor:t1",              
  "PRM:/r6/s1-Sensor:s1",              
  // Tail sensors
  "PRM:/r6/s2-Sensor:s2",              
  "PRM:/r6/s3-Sensor:s3",              
  "PRM:/r6/s4-Sensor:s4",              
  // Body sensors
  "PRM:/a1-Sensor:a1",                 
  "PRM:/a2-Sensor:a2",                 
  "PRM:/a3-Sensor:a3"                  
};

class AiboSensor{
  
  // METHODS
 public: 
  AiboSensor();

  virtual ~AiboSensor(){};

  
  bool Initialized() const;
  
  void InitSensorIndex(OSensorFrameVectorData* sensorVec);


  void PrintSensorValues(OSensorFrameVectorData* sensorVec);
  
  void PrintSensorValue(OSensorFrameVectorData* sensorVec, SENSOR sens);

  void PrintJointValue(OSensorFrameVectorData* sensorVec, JOINT_SENSOR joint);


  void PrintSeparator() const;
  
  slongword GetSensorValue(OSensorFrameVectorData* sensorVec, SENSOR sens);

  slongword GetMeanSensorValue(OSensorFrameVectorData* sensorVec, SENSOR index, int numFrames);
  

   double GetDistance(OSensorFrameVectorData* sensorVec);

   double GetMeanDistance(OSensorFrameVectorData* sensorVec, int numFrames);
  
  
   double GetJointValue(OSensorFrameVectorData* sensorVec, JOINT_SENSOR joint);

   double GetMeanJointValue(OSensorFrameVectorData* sensorVec, JOINT_SENSOR joint, int numFrames);

  double GetRefJointValue(OSensorFrameVectorData* sensorVec, JOINT_SENSOR joint);

   double GetMeanRefJointValue(OSensorFrameVectorData* sensorVec, JOINT_SENSOR joint, int numFrames);


   double GetTemp(OSensorFrameVectorData* sensorVec);

   double GetMeanTemp(OSensorFrameVectorData* sensorVec, int numFrames);


   double GetXAccel(OSensorFrameVectorData* sensorVec);

   double GetMeanXAccel(OSensorFrameVectorData* sensorVec, int numFrames);


   double GetYAccel(OSensorFrameVectorData* sensorVec);

   double GetMeanYAccel(OSensorFrameVectorData* sensorVec, int numFrames);


   double GetZAccel(OSensorFrameVectorData* sensorVec);
  

   double GetMeanZAccel(OSensorFrameVectorData* sensorVec, int numFrames);
  
   bool IsPressed(OSensorFrameVectorData* sensorVec, SWITCH sw);



  OPrimitiveID GetPrimitiveID(SENSOR sensor) const;

 protected:
  
  bool         SensorIndexInitialized;

  int          SensorIndex[NUM_SENSORS];

  OPrimitiveID SensorID[NUM_SENSORS];
  
};


// ******************************************
// **                                      **
// ******************************************
inline bool AiboSensor::Initialized() const
{
  return SensorIndexInitialized;
};


// ******************************************
// **                                      **
// ******************************************
inline OPrimitiveID AiboSensor::GetPrimitiveID(SENSOR sensor) const
{
  return SensorID[sensor];
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetDistance(OSensorFrameVectorData* sensorVec)
{ 
  return ((double)GetSensorValue(sensorVec, PSD)/1000.0);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanDistance(OSensorFrameVectorData* sensorVec, 
                                         int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, PSD, numFrames)/1000.0);
};

  
// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetJointValue(OSensorFrameVectorData* sensorVec,
                                       JOINT_SENSOR joint)
{
  return ((double)GetSensorValue(sensorVec, SENSOR(joint))/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanJointValue(OSensorFrameVectorData* sensorVec, 
                                           JOINT_SENSOR joint, 
                                           int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, SENSOR(joint), numFrames)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetTemp(OSensorFrameVectorData* sensorVec)
{
   return ((double)GetSensorValue(sensorVec, THERMO)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanTemp(OSensorFrameVectorData* sensorVec, 
                                     int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, THERMO, numFrames)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetXAccel(OSensorFrameVectorData* sensorVec)
{
   return ((double)GetSensorValue(sensorVec, ACC_X)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanXAccel(OSensorFrameVectorData* sensorVec,
                                       int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, ACC_X, numFrames)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetYAccel(OSensorFrameVectorData* sensorVec)
{
  return ((double)GetSensorValue(sensorVec, ACC_Y)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanYAccel(OSensorFrameVectorData* sensorVec, 
                                       int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, ACC_Y, numFrames)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetZAccel(OSensorFrameVectorData* sensorVec)
{
  return ((double)GetSensorValue(sensorVec, ACC_Z)/MICRO);
};

// ******************************************
// **                                      **
// ******************************************
inline double AiboSensor::GetMeanZAccel(OSensorFrameVectorData* sensorVec, 
                                       int numFrames)
{
  return ((double)GetMeanSensorValue(sensorVec, ACC_Z, numFrames)/MICRO);
};


// ******************************************
// **                                      **
// ******************************************
inline bool AiboSensor::IsPressed(OSensorFrameVectorData* sensorVec, 
                                  SWITCH sw)
{
  return (GetSensorValue(sensorVec, SENSOR(sw)));
};


#endif  //define AIBOSENSOR_H

---

Tim Baier AiboLib v0.2.4

Generated Thu Jan 19 11:54:29 2006 by 1.4.3