Vince et Yann / Mbed 2 deprecated APP1_customProtocole

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of APP1_customProtocole by Yann Lemay-Sévigny

acceleroMMA8452Q.cpp

Committer:
yannolecool
Date:
2016-01-10
Revision:
3:b3574c385012
Child:
4:e6df056992c1

File content as of revision 3:b3574c385012:

#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);
}