Update ecrobot to nxtOSEK_v212.zip

[nxt-jsp/etrobo-atk.git] / nxtOSEK / ecrobot / c++ / device / AccelSensor.cpp

diff --git a/nxtOSEK/ecrobot/c++/device/AccelSensor.cpp b/nxtOSEK/ecrobot/c++/device/AccelSensor.cpp
new file mode 100644 (file)
index 0000000..9cbef88
--- /dev/null
+++ b/nxtOSEK/ecrobot/c++/device/AccelSensor.cpp
@@ -0,0 +1,37 @@
+//\r
+// AccelSensor.cpp\r
+//\r
+// Copyright 2009 by Takashi Chikamasa, Jon C. Martin and Robert W. Kramer\r
+//\r
+\r
+#include "AccelSensor.h"\r
+using namespace ecrobot;\r
+\r
+\r
+//=============================================================================\r
+// Constructor\r
+AccelSensor::AccelSensor(ePortS port)\r
+:\r
+mI2c(I2c(port)) // activate I2C\r
+{}\r
+\r
+//=============================================================================\r
+// get acceleration in 3 axes(x/y/z)\r
+void AccelSensor::getAccel(S16 axes[3]) const\r
+{\r
+       U8 data[6];\r
+\r
+       this->get(data);\r
+\r
+       for (SINT i=0; i<3; i++)\r
+       {\r
+               axes[i] = static_cast<S16>(data[i]);\r
+               if (axes[i] > 127)\r
+               {\r
+                       axes[i] -= 256;\r
+               }\r
+               /* convert to 10 bit value */\r
+               axes[i] = ((axes[i] << 2) | (static_cast<S16>(data[i+3]) & 0x0003));\r
+       }\r
+}\r
+\r