Diff: acceleroMMA8452Q.cpp

Revision:

3:b3574c385012

Child:

4:e6df056992c1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/acceleroMMA8452Q.cpp	Sun Jan 10 21:55:39 2016 +0000
@@ -0,0 +1,87 @@
+#include "acceleroMMA8452Q.h"
+#include "math.h"
+
+#define M_PI 3.14159265358979323846
+
+Accelero::Accelero(int frequency): i2cPort(p28, p27)
+{
+    init(frequency);
+}
+
+void Accelero::init(int frequency)
+{
+    char command[2];
+    command[0] = 0x2B;
+    command[1] = 0x40;
+    
+    i2cPort.write(ACCELERO_WRITE_ADRESS, command, 1, false);
+    wait(0.1);
+    
+    char response = 0x00;
+    i2cPort.frequency(frequency);
+    i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_WHO_AM_I, 1, true);
+    i2cPort.read(ACCELERO_READ_ADRESS, &response, 1, false);
+    
+    if(response == ACCELERO_RESPONSE_WHO_AM_I)
+    {
+        DigitalOut led4(LED4);
+        led4 = 1;
+    }
+    
+    command[0] = ACCELERO_REGISTER_CTRL_REG1;
+    command[1] = 0x01; //Put accelro in active mode
+    
+    i2cPort.write(ACCELERO_WRITE_ADRESS, command, 1, false);
+}
+
+vector Accelero::getAccelVector()
+{
+    char data[6];
+    i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_OUT_X_MSB, 1, true);
+    i2cPort.read(ACCELERO_READ_ADRESS,  data, 6, false);
+    
+    vector accelVector;
+    
+    //Transform data to be really signed
+    int tempData;
+    tempData = (data[0] << 4) + (data[1] >> 4);
+    if(tempData >= 2048)
+    {
+        tempData |= 0xFFFFF000;
+    }
+    accelVector.x = tempData;
+    
+    tempData = (data[2] << 4) + (data[3] >> 4);
+    if(tempData >= 2048)
+    {
+        tempData |= 0xFFFFF000;
+    }
+    accelVector.y = tempData;
+    
+    tempData = (data[4] << 4) + (data[5] >> 4);
+    if(tempData >= 2048)
+    {
+        tempData |= 0xFFFFF000;
+    }
+    accelVector.z = tempData;
+    
+    return accelVector;
+}
+    
+int Accelero::getAngle()
+{
+    vector accelVector = getAccelVector();
+    
+    int denominateur = accelVector.x*accelVector.x + accelVector.y*accelVector.y + accelVector.z*accelVector.z;
+    
+    float result = acos(abs(accelVector.z)/sqrtf(denominateur));
+    result = result * 180 / M_PI;
+    result = result * 100;
+    
+    return int(result);
+}
+    
+    
+    
+    
+    
\ No newline at end of file