Diff: main.cpp

Revision:

2:b878d61bf57b

Parent:

0:44f624c5501e

Child:

3:68f55e42c85f

--- a/main.cpp	Fri Dec 18 14:40:54 2015 +0000
+++ b/main.cpp	Thu Feb 16 11:43:51 2017 +0000
@@ -1,25 +1,107 @@
 #include "mbed.h"
 #include "GYRO_DISCO_F429ZI.h"
+#include "LCD_DISCO_F429ZI.h"
+#include "stm32f429i_discovery_lcd.h"
 
 GYRO_DISCO_F429ZI gyro;
+LCD_DISCO_F429ZI lcd;
 
 DigitalOut led1(LED1);
 
 int main()
 {
     float GyroBuffer[3];
-  
-    printf("Gyroscope started\n");
-  
+    float zero_level[3] = {0, 0, 0};
+    uint8_t buffer[100];
+    lcd.Init();
+    lcd.DisplayStringAt(10, 10, (uint8_t*)"Gyroscope_started", CENTER_MODE);
+    uint8_t tmpreg = 0;
+    GYRO_IO_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);
+    switch(tmpreg & L3GD20_FULLSCALE_SELECTION)
+    {
+        case L3GD20_FULLSCALE_250:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"250", CENTER_MODE);
+        break;
+    
+        case L3GD20_FULLSCALE_500:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"500", CENTER_MODE);
+        break;
+    
+        case L3GD20_FULLSCALE_2000:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"2000", CENTER_MODE);
+        break;
+  }
+    GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG1_ADDR, 1);
+    switch(tmpreg >> 6) {
+        case 0:
+            lcd.DisplayStringAt(20, 40, (uint8_t*)"95", CENTER_MODE);
+            break;
+        case 1:
+            lcd.DisplayStringAt(20, 40, (uint8_t*)"190", CENTER_MODE);
+            break;
+        case 2:
+            lcd.DisplayStringAt(20, 40, (uint8_t*)"380", CENTER_MODE);
+            break;
+        case 3:
+            lcd.DisplayStringAt(20, 40, (uint8_t*)"760", CENTER_MODE);
+            break;
+    }
+    //BSP_LCD_Init();
+    //BSP_LCD_De
+    //BSP_LCD_DisplayStringAt(10,10, (uint8_t*)"Gyroscope_started", CENTER_MODE);
+    
+    for (int i = 0; i < 50; i++) {
+        gyro.GetXYZ(GyroBuffer);
+        zero_level[0] += GyroBuffer[0] / 17.5f;
+        zero_level[1] += GyroBuffer[1] / 17.5f;
+        zero_level[2] += GyroBuffer[2] / 17.5f;
+        //zero_level[0] += GyroBuffer[0];
+        //zero_level[1] += GyroBuffer[1];
+        //zero_level[2] += GyroBuffer[2];
+        wait_ms(10);
+    }
+    for (int i = 0; i < 3; i++)
+        zero_level[i] /= 50;
+    
+    float noise_level = 0.0;
+    for (int i = 0; i < 50; i++) {
+        gyro.GetXYZ(GyroBuffer);
+        if ((GyroBuffer[0] / 17.5f - zero_level[0]) > noise_level) {
+            noise_level = (GyroBuffer[0] / 17.5f - zero_level[0]);
+        } 
+        else if ((GyroBuffer[0] / 17.5f - zero_level[0]) < -noise_level) {
+            noise_level = -(GyroBuffer[0] / 17.5f - zero_level[0]);
+        }
+        wait_ms(10);
+    }
+    sprintf((char*)buffer, "Noise = %.2f\n", noise_level);
+    lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
+    float angle = 0.0, prev_value = 0.0;
     while(1) {
         // Read Gyroscope values
+        wait(0.02);
+        //GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1);
+        //while((tmpreg >> 3) & 0x01 != 1) GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1);
         gyro.GetXYZ(GyroBuffer);
+        float value = GyroBuffer[0] / 17.5f - zero_level[0];
+        if (value >= noise_level || value <= -noise_level) {
+            angle += (value + prev_value) / 2 * 0.0175 * 0.02f / 0.825;
+        }
+        prev_value = value;
+        sprintf((char*)buffer, "Angle = %.3f\n", angle);
+        lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
         // Display values      
-        printf("X = %f\n", GyroBuffer[0]);
-        printf("Y = %f\n", GyroBuffer[1]);
-        printf("Z = %f\n", GyroBuffer[2]);
-        printf("\033[3A"); // Moves cursor up x lines (x value is between [ and A)
-        led1 = !led1;
-        wait(1);
+        //sprintf((char*)buffer, "X = %.2f\n", (GyroBuffer[0] - zero_level[0]) / 1000);
+        //sprintf((char*)buffer, "X = %.2f\n", GyroBuffer[0] / 17.5f - zero_level[0]);
+        //lcd.DisplayStringAt(20, 10, buffer, CENTER_MODE);
+        //sprintf((char*)buffer, "Y = %.2f\n", (GyroBuffer[1] - zero_level[1]) / 1000);
+        //sprintf((char*)buffer, "Y = %.2f\n", GyroBuffer[1] / 17.5f - zero_level[1]);
+        //lcd.DisplayStringAt(20, 40, buffer, CENTER_MODE);
+        //sprintf((char*)buffer, "Z = %.2f\n", (GyroBuffer[2] - zero_level[2]) / 1000);
+        //sprintf((char*)buffer, "Z = %.2f\n", GyroBuffer[2] / 17.5f - zero_level[2]);
+        //lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
+        //led1 = !led1;
+        //wait(1);
+        //lcd.Clear(0xffffffff);
     }
 }