Diff: BMI160.cpp

Revision:

2:f9ddabfe2eb6

Parent:

1:55d35606b477

--- a/BMI160.cpp	Tue Oct 18 17:47:44 2016 +0000
+++ b/BMI160.cpp	Wed Oct 19 20:44:59 2016 +0000
@@ -11,25 +11,21 @@
 
 /* buffer to store acc samples */
 int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
- 
 double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
- 
 double accel_ang_x, accel_ang_y;
  
 char i2c_reg_buffer[2] = {0};
 
-//Instancia o LCD
-ST7567 lcd(D11, D13, D12, D9, D10); // mosi, sclk, reset, A0, nCS
-
  /*Comunicacao I2C*/
 I2C i2c(P2_3, P2_4);
 
+int currentState;
+int previousState;
 
-void BMI160::lcdClear(){
-    for(int i=0;i<5;i++)
-        lcd.printf("\n");
-}
 void BMI160::configureAccelerometer(){
+    currentState = 0;
+    previousState = -1;
+
     
     /*Config Freq. I2C Bus*/
     i2c.frequency(20000);
@@ -38,56 +34,70 @@
     i2c_reg_buffer[0] = 0x7E;
     i2c_reg_buffer[1] = 0xB6;    
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
-    wait_ms(200);
-    //lcd.printf("BMI160 Resetado\n\r");
-    wait_ms(100);
-    
+
     /*Habilita o Acelerometro*/
     i2c_reg_buffer[0] = 0x7E;
     i2c_reg_buffer[1] = 0x11; //PMU Normal   
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
-    //lcd.printf("Acc Habilitado\n\r");
-
+    
     /*Config o Data Rate ACC em 1600Hz*/
     i2c_reg_buffer[0] = 0x40;
     i2c_reg_buffer[1] = 0x2C;    
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
-    //lcd.printf("Data Rate ACC Selecionado a 1600Hz\n\r");
     
 }
 
-void BMI160::calculateMovement(){  
-        
+void BMI160::readAccelerometer(){
+
+    /*Le os Registradores do Acelerometro*/
+    i2c_reg_buffer[0] = 0x12;
+    i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
+    i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
     /*Ajusta dados brutos Acelerometro em unidades de g */
     acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0);
     acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0);
     acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
                 
     /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
-    accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
-   // accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
-  
-  
-    /*Imprime os dados ACC pre-formatados*/
-  //  lcd.printf("Acc(G): %.3f,%.3f,%.3f\n\r",acc_result_buffer[0], acc_result_buffer[1], acc_result_buffer[2]);
-    lcd.printf("Angles(x,y): %.3f\n\r", accel_ang_x);
-    wait_ms(3000);
-    lcdClear();
+    accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
+
 }
 
-void BMI160::readAccelerometer(){
-    
-    while(1) { 
-        /*Le os Registradores do Acelerometro*/
-        i2c_reg_buffer[0] = 0x12;
-        i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
-        i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
-        calculateMovement();
-    }
+int BMI160::getAngle(){
+
+        readAccelerometer();
+        
+        if(currentState == 0 && accel_ang_y < -20){
+            currentState = 1;
+            previousState = 0;    
+        }
+        else if(currentState == 0 && accel_ang_y > 20){
+            currentState = 2;
+            previousState = 0;
+        }
+        else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
+            currentState = 0;
+            previousState = 1;  
+        }  
+        else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
+            currentState = 0;
+            previousState = 1;  
+        } 
+        else if(currentState == 2 && accel_ang_y > -20 && accel_ang_y < 20){
+            currentState = 0;
+            previousState = 2;
+        }
+        
+        if(currentState == 1 && previousState == 0) 
+            return 90;
+        else if(currentState == 2 && previousState == 0)
+            return 270;
+        else if(currentState == 0 && previousState == 1)
+            return 0;
+        else if(currentState == 0 && previousState == 2)
+            return 180;
+        else
+            return -1;
+        
 }
 
-void BMI160::runTempCommands(){
-    configureAccelerometer();
-    readAccelerometer();
-    
-}