nxtOSEK/c++/src/AccelSensor.cc

   1 //
   2 // AccelSensor.cc
   3 //
   4 // Hi-Technic Acceleration / Tilt sensor class
   5 //
   6 // Written 12-jan-2008 by rwk
   7 //
   8 // Copyright 2007, 2008 by Robert W. Kramer and Takashi Chikamasa
   9 //
  10
  11 #include "AccelSensor.h"
  12
  13 //=============================================================================
  14 // AccelSensor::AccelSensor(unsigned char _port)
  15 //   constructor for sensor object
  16 //
  17 // Parm
  18 //   _port - port number sensor is connected to
  19 //
  20
  21 AccelSensor::AccelSensor(unsigned char _port) {
  22
  23   //
  24   // remember the port number for data transfers
  25   //
  26
  27   port = _port;
  28
  29   //
  30   // turn on the sensor port. This comes from Takashi's code
  31   //
  32
  33   nxt_avr_set_input_power(port,POWER_ON);
  34   i2c_enable(port);
  35
  36   //
  37   // we are now activated (avoids redundant calls later)
  38   //
  39
  40   activated = true;
  41 }
  42
  43
  44
  45 //=============================================================================
  46 // AccelSensor::~AccelSensor(void)
  47 //   class destructor
  48 //
  49
  50 AccelSensor::~AccelSensor(void) {
  51
  52   //
  53   // turn off power. Not sure if this is needed here.
  54   //
  55
  56   nxt_avr_set_input_power(port,POWER_OFF);
  57
  58   //
  59   // disable the port. This is necessary.
  60   //
  61
  62   i2c_disable(port);
  63 }
  64
  65
  66
  67 //=============================================================================
  68 // void AccelSensor::activate(void)
  69 //   turn on the sensor port
  70 //
  71
  72 void AccelSensor::activate(void) {
  73
  74   //
  75   // nothing to do if the port's already on
  76   //
  77
  78   if (activated)
  79     return;
  80
  81   //
  82   // turn on the port and power
  83   //
  84
  85   nxt_avr_set_input_power(port,POWER_ON);
  86   i2c_enable(port);
  87
  88   //
  89   // remember that we've done this
  90   //
  91
  92   activated = true;
  93 }
  94
  95
  96
  97 //=============================================================================
  98 // void AccelSensor::passivate(void)
  99 //   turn off the port
 100 //
 101
 102 void AccelSensor::passivate(void) {
 103
 104   //
 105   // nothing to do if the port's off
 106   //
 107
 108   if (!activated)
 109     return;
 110
 111   //
 112   // turn off the port. As with the destructor, I'm not sure if the first call
 113   // is needed.
 114   //
 115
 116   nxt_avr_set_input_power(port,POWER_OFF);
 117   i2c_disable(port);
 118
 119   //
 120   // remember what we did
 121   //
 122
 123   activated = false;
 124 }
 125
 126
 127
 128 //=============================================================================
 129 // void AccelSensor::fetchData(short int axes[3])
 130 //   fetch acceleration data from the sensor
 131 //
 132 // Parm
 133 //   axes - array to hold acceleration data along the axes
 134 //
 135
 136 void AccelSensor::fetchData(short int axes[3]) {
 137   int i;
 138   unsigned char inBuf[6];
 139
 140   //
 141   // Data representation from the sensor, per Hi-Technic's documentation:
 142   //
 143   // data[0]: X axis upper 8 bits
 144   // data[1]: Y axis upper 8 bits
 145   // data[2]: Z axis upper 8 bits
 146   // data[3]: X axis lower 2 bits
 147   // data[4]: Y axis lower 2 bits
 148   // data[5]: Z axis lower 2 bits
 149   //
 150
 151   //
 152   // wait for port to become available
 153   //
 154
 155   if (i2c_busy(port))
 156     sensorSleep(port);
 157
 158   //
 159   // initiate data transfer
 160   //
 161
 162   i2c_start_transaction(port,1,0x42,1,inBuf,6,0);
 163
 164   //
 165   // wait for port to complete the transfer
 166   //
 167
 168   if (i2c_busy(port))
 169     sensorSleep(port);
 170
 171   //
 172   // convert raw data into final results
 173   //
 174
 175   for (i=0; i<3; i++) {
 176     axes[i] = (S16)inBuf[i];
 177     if (axes[i] > 127) axes[i] -= 256;
 178     /* convert to 10 bit value */
 179     axes[i] = (axes[i] << 2) | ((S16)inBuf[i+3] & 0x0003);
 180   }
 181 }
 182
 183
 184
 185 //=============================================================================
 186 // short int AccelSensor::getXAccel(void)
 187 //   get acceleration on X axis
 188 //
 189 // NB: positive is forward. 200 ticks per g.
 190 //
 191
 192 short int AccelSensor::getXAccel(void) {
 193   short int axes[3];
 194
 195   fetchData(axes);
 196
 197   return axes[0];
 198 }
 199
 200
 201
 202 //=============================================================================
 203 // short int AccelSensor::getYAccel(void)
 204 //   get acceleration on Y axis
 205 //
 206 // NB: positive is left. 200 ticks per g.
 207 //
 208
 209 short int AccelSensor::getYAccel(void) {
 210   short int axes[3];
 211
 212   fetchData(axes);
 213
 214   return axes[1];
 215 }
 216
 217
 218
 219 //=============================================================================
 220 // short int AccelSensor::getZAccel(void)
 221 //   get acceleration on Z axis
 222 //
 223 // NB: positive is up. 200 ticks per g.
 224 //
 225
 226 short int AccelSensor::getZAccel(void) {
 227   short int axes[3];
 228
 229   fetchData(axes);
 230
 231   return axes[2];
 232 }
 233
 234
 235
 236 //=============================================================================
 237 // void AccelSensor::getAccel(short int axes[3])
 238 //   get acceleration on all three axes
 239 //
 240
 241 void AccelSensor::getAccel(short int axes[3]) {
 242
 243   fetchData(axes);
 244
 245 }
 246
 247
 248
 249 //=============================================================================
 250 // void AccelSensor::getAccel(short int &x,short int &y,short int &z) {
 251 //  get acceleration on all three axes
 252 //
 253
 254 void AccelSensor::getAccel(short int &x,short int &y,short int &z) {
 255   short int axes[3];
 256
 257   fetchData(axes);
 258
 259   x = axes[0];
 260   y = axes[1];
 261   z = axes[2];
 262 }