Josias Orlando / sysclean_BMI160

Library for accelerometer and SysClean

BMI160.cpp@2:f9ddabfe2eb6, 2016-10-19 (annotated)

Committer:
jotamo
Date:
Wed Oct 19 20:44:59 2016 +0000
Revision:
2:f9ddabfe2eb6
Parent:
1:55d35606b477 
Final version of BMI160 accelerometer library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jotamo 1:55d35606b477 1 #include "BMI160.h"
jotamo 1:55d35606b477 2
jotamo 1:55d35606b477 3 /* defines the axis for acc */
jotamo 1:55d35606b477 4 #define ACC_NOOF_AXIS 3
jotamo 1:55d35606b477 5
jotamo 1:55d35606b477 6 /* bmi160 slave address */
jotamo 1:55d35606b477 7 #define BMI160_ADDR ((0x68)<<1)
jotamo 1:55d35606b477 8
jotamo 1:55d35606b477 9 /*Valor para Transformar de Radiano para Graus*/
jotamo 1:55d35606b477 10 #define RAD_DEG 57.29577951
jotamo 1:55d35606b477 11
jotamo 1:55d35606b477 12 /* buffer to store acc samples */
jotamo 1:55d35606b477 13 int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
jotamo 1:55d35606b477 14 double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
jotamo 1:55d35606b477 15 double accel_ang_x, accel_ang_y;
jotamo 1:55d35606b477 16
jotamo 1:55d35606b477 17 char i2c_reg_buffer[2] = {0};
jotamo 1:55d35606b477 18
jotamo 1:55d35606b477 19 /*Comunicacao I2C*/
jotamo 1:55d35606b477 20 I2C i2c(P2_3, P2_4);
jotamo 1:55d35606b477 21
jotamo 2:f9ddabfe2eb6 22 int currentState;
jotamo 2:f9ddabfe2eb6 23 int previousState;
jotamo 1:55d35606b477 24
jotamo 1:55d35606b477 25 void BMI160::configureAccelerometer(){
jotamo 2:f9ddabfe2eb6 26 currentState = 0;
jotamo 2:f9ddabfe2eb6 27 previousState = -1;
jotamo 2:f9ddabfe2eb6 28
jotamo 1:55d35606b477 29
jotamo 1:55d35606b477 30 /*Config Freq. I2C Bus*/
jotamo 1:55d35606b477 31 i2c.frequency(20000);
jotamo 1:55d35606b477 32
jotamo 1:55d35606b477 33 /*Reset BMI160*/
jotamo 1:55d35606b477 34 i2c_reg_buffer[0] = 0x7E;
jotamo 1:55d35606b477 35 i2c_reg_buffer[1] = 0xB6;
jotamo 1:55d35606b477 36 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
jotamo 2:f9ddabfe2eb6 37
jotamo 1:55d35606b477 38 /*Habilita o Acelerometro*/
jotamo 1:55d35606b477 39 i2c_reg_buffer[0] = 0x7E;
jotamo 1:55d35606b477 40 i2c_reg_buffer[1] = 0x11; //PMU Normal
jotamo 1:55d35606b477 41 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
jotamo 2:f9ddabfe2eb6 42
jotamo 1:55d35606b477 43 /*Config o Data Rate ACC em 1600Hz*/
jotamo 1:55d35606b477 44 i2c_reg_buffer[0] = 0x40;
jotamo 1:55d35606b477 45 i2c_reg_buffer[1] = 0x2C;
jotamo 1:55d35606b477 46 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
jotamo 1:55d35606b477 47
jotamo 1:55d35606b477 48 }
jotamo 1:55d35606b477 49
jotamo 2:f9ddabfe2eb6 50 void BMI160::readAccelerometer(){
jotamo 2:f9ddabfe2eb6 51
jotamo 2:f9ddabfe2eb6 52 /*Le os Registradores do Acelerometro*/
jotamo 2:f9ddabfe2eb6 53 i2c_reg_buffer[0] = 0x12;
jotamo 2:f9ddabfe2eb6 54 i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
jotamo 2:f9ddabfe2eb6 55 i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
jotamo 1:55d35606b477 56 /*Ajusta dados brutos Acelerometro em unidades de g */
jotamo 1:55d35606b477 57 acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0);
jotamo 1:55d35606b477 58 acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0);
jotamo 1:55d35606b477 59 acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
jotamo 1:55d35606b477 60
jotamo 1:55d35606b477 61 /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
jotamo 2:f9ddabfe2eb6 62 accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
jotamo 2:f9ddabfe2eb6 63
jotamo 1:55d35606b477 64 }
jotamo 1:55d35606b477 65
jotamo 2:f9ddabfe2eb6 66 int BMI160::getAngle(){
jotamo 2:f9ddabfe2eb6 67
jotamo 2:f9ddabfe2eb6 68 readAccelerometer();
jotamo 2:f9ddabfe2eb6 69
jotamo 2:f9ddabfe2eb6 70 if(currentState == 0 && accel_ang_y < -20){
jotamo 2:f9ddabfe2eb6 71 currentState = 1;
jotamo 2:f9ddabfe2eb6 72 previousState = 0;
jotamo 2:f9ddabfe2eb6 73 }
jotamo 2:f9ddabfe2eb6 74 else if(currentState == 0 && accel_ang_y > 20){
jotamo 2:f9ddabfe2eb6 75 currentState = 2;
jotamo 2:f9ddabfe2eb6 76 previousState = 0;
jotamo 2:f9ddabfe2eb6 77 }
jotamo 2:f9ddabfe2eb6 78 else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
jotamo 2:f9ddabfe2eb6 79 currentState = 0;
jotamo 2:f9ddabfe2eb6 80 previousState = 1;
jotamo 2:f9ddabfe2eb6 81 }
jotamo 2:f9ddabfe2eb6 82 else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
jotamo 2:f9ddabfe2eb6 83 currentState = 0;
jotamo 2:f9ddabfe2eb6 84 previousState = 1;
jotamo 2:f9ddabfe2eb6 85 }
jotamo 2:f9ddabfe2eb6 86 else if(currentState == 2 && accel_ang_y > -20 && accel_ang_y < 20){
jotamo 2:f9ddabfe2eb6 87 currentState = 0;
jotamo 2:f9ddabfe2eb6 88 previousState = 2;
jotamo 2:f9ddabfe2eb6 89 }
jotamo 2:f9ddabfe2eb6 90
jotamo 2:f9ddabfe2eb6 91 if(currentState == 1 && previousState == 0)
jotamo 2:f9ddabfe2eb6 92 return 90;
jotamo 2:f9ddabfe2eb6 93 else if(currentState == 2 && previousState == 0)
jotamo 2:f9ddabfe2eb6 94 return 270;
jotamo 2:f9ddabfe2eb6 95 else if(currentState == 0 && previousState == 1)
jotamo 2:f9ddabfe2eb6 96 return 0;
jotamo 2:f9ddabfe2eb6 97 else if(currentState == 0 && previousState == 2)
jotamo 2:f9ddabfe2eb6 98 return 180;
jotamo 2:f9ddabfe2eb6 99 else
jotamo 2:f9ddabfe2eb6 100 return -1;
jotamo 2:f9ddabfe2eb6 101
jotamo 1:55d35606b477 102 }
jotamo 1:55d35606b477 103