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@3:b3574c385012, 2016-01-10 (annotated)

Committer:
yannolecool
Date:
Sun Jan 10 21:55:39 2016 +0000
Revision:
3:b3574c385012
Child:
4:e6df056992c1
Version fonctionnel de l'APP (non optimiser)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yannolecool 3:b3574c385012 1 #include "acceleroMMA8452Q.h"
yannolecool 3:b3574c385012 2 #include "math.h"
yannolecool 3:b3574c385012 3
yannolecool 3:b3574c385012 4 #define M_PI 3.14159265358979323846
yannolecool 3:b3574c385012 5
yannolecool 3:b3574c385012 6 Accelero::Accelero(int frequency): i2cPort(p28, p27)
yannolecool 3:b3574c385012 7 {
yannolecool 3:b3574c385012 8 init(frequency);
yannolecool 3:b3574c385012 9 }
yannolecool 3:b3574c385012 10
yannolecool 3:b3574c385012 11 void Accelero::init(int frequency)
yannolecool 3:b3574c385012 12 {
yannolecool 3:b3574c385012 13 char command[2];
yannolecool 3:b3574c385012 14 command[0] = 0x2B;
yannolecool 3:b3574c385012 15 command[1] = 0x40;
yannolecool 3:b3574c385012 16
yannolecool 3:b3574c385012 17 i2cPort.write(ACCELERO_WRITE_ADRESS, command, 1, false);
yannolecool 3:b3574c385012 18 wait(0.1);
yannolecool 3:b3574c385012 19
yannolecool 3:b3574c385012 20 char response = 0x00;
yannolecool 3:b3574c385012 21 i2cPort.frequency(frequency);
yannolecool 3:b3574c385012 22 i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_WHO_AM_I, 1, true);
yannolecool 3:b3574c385012 23 i2cPort.read(ACCELERO_READ_ADRESS, &response, 1, false);
yannolecool 3:b3574c385012 24
yannolecool 3:b3574c385012 25 if(response == ACCELERO_RESPONSE_WHO_AM_I)
yannolecool 3:b3574c385012 26 {
yannolecool 3:b3574c385012 27 DigitalOut led4(LED4);
yannolecool 3:b3574c385012 28 led4 = 1;
yannolecool 3:b3574c385012 29 }
yannolecool 3:b3574c385012 30
yannolecool 3:b3574c385012 31 command[0] = ACCELERO_REGISTER_CTRL_REG1;
yannolecool 3:b3574c385012 32 command[1] = 0x01; //Put accelro in active mode
yannolecool 3:b3574c385012 33
yannolecool 3:b3574c385012 34 i2cPort.write(ACCELERO_WRITE_ADRESS, command, 1, false);
yannolecool 3:b3574c385012 35 }
yannolecool 3:b3574c385012 36
yannolecool 3:b3574c385012 37 vector Accelero::getAccelVector()
yannolecool 3:b3574c385012 38 {
yannolecool 3:b3574c385012 39 char data[6];
yannolecool 3:b3574c385012 40 i2cPort.write(ACCELERO_WRITE_ADRESS, &ACCELERO_REGISTER_OUT_X_MSB, 1, true);
yannolecool 3:b3574c385012 41 i2cPort.read(ACCELERO_READ_ADRESS, data, 6, false);
yannolecool 3:b3574c385012 42
yannolecool 3:b3574c385012 43 vector accelVector;
yannolecool 3:b3574c385012 44
yannolecool 3:b3574c385012 45 //Transform data to be really signed
yannolecool 3:b3574c385012 46 int tempData;
yannolecool 3:b3574c385012 47 tempData = (data[0] << 4) + (data[1] >> 4);
yannolecool 3:b3574c385012 48 if(tempData >= 2048)
yannolecool 3:b3574c385012 49 {
yannolecool 3:b3574c385012 50 tempData |= 0xFFFFF000;
yannolecool 3:b3574c385012 51 }
yannolecool 3:b3574c385012 52 accelVector.x = tempData;
yannolecool 3:b3574c385012 53
yannolecool 3:b3574c385012 54 tempData = (data[2] << 4) + (data[3] >> 4);
yannolecool 3:b3574c385012 55 if(tempData >= 2048)
yannolecool 3:b3574c385012 56 {
yannolecool 3:b3574c385012 57 tempData |= 0xFFFFF000;
yannolecool 3:b3574c385012 58 }
yannolecool 3:b3574c385012 59 accelVector.y = tempData;
yannolecool 3:b3574c385012 60
yannolecool 3:b3574c385012 61 tempData = (data[4] << 4) + (data[5] >> 4);
yannolecool 3:b3574c385012 62 if(tempData >= 2048)
yannolecool 3:b3574c385012 63 {
yannolecool 3:b3574c385012 64 tempData |= 0xFFFFF000;
yannolecool 3:b3574c385012 65 }
yannolecool 3:b3574c385012 66 accelVector.z = tempData;
yannolecool 3:b3574c385012 67
yannolecool 3:b3574c385012 68 return accelVector;
yannolecool 3:b3574c385012 69 }
yannolecool 3:b3574c385012 70
yannolecool 3:b3574c385012 71 int Accelero::getAngle()
yannolecool 3:b3574c385012 72 {
yannolecool 3:b3574c385012 73 vector accelVector = getAccelVector();
yannolecool 3:b3574c385012 74
yannolecool 3:b3574c385012 75 int denominateur = accelVector.x*accelVector.x + accelVector.y*accelVector.y + accelVector.z*accelVector.z;
yannolecool 3:b3574c385012 76
yannolecool 3:b3574c385012 77 float result = acos(abs(accelVector.z)/sqrtf(denominateur));
yannolecool 3:b3574c385012 78 result = result * 180 / M_PI;
yannolecool 3:b3574c385012 79 result = result * 100;
yannolecool 3:b3574c385012 80
yannolecool 3:b3574c385012 81 return int(result);
yannolecool 3:b3574c385012 82 }
yannolecool 3:b3574c385012 83
yannolecool 3:b3574c385012 84
yannolecool 3:b3574c385012 85
yannolecool 3:b3574c385012 86
yannolecool 3:b3574c385012 87