Diff: main.cpp

Revision:

0:a3739a2e895d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 13 10:28:00 2019 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "LCD_DISCO_F746NG.h"
+#include "stm32746g_discovery_lcd.h"
+
+LCD_DISCO_F746NG lcd;
+DigitalOut myled(LED1);
+MPU6050 mpu(D14, D15);
+
+int main()
+{
+    float GyroBuffer[3];
+    float zero_level[3] = {0, 0, 0};
+    uint8_t buffer[100];
+    lcd.Init();
+    lcd.DisplayStringAt(10, 10, (uint8_t*)"Gyros_star", CENTER_MODE);
+    uint8_t tmpreg = 0;
+    read(MPU6050_GYRO_CONFIG_REG,&tmpreg,1);
+    switch(tmpreg & MPU6050_GYRO_XOUT_H_REG)
+    {
+        case MPU6050_GYRO_RANGE_250:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"250", CENTER_MODE);
+        break;
+    
+        case MPU6050_GYRO_RANGE_500:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"500", CENTER_MODE);
+        break;
+    
+        case MPU6050_GYRO_RANGE_2000:
+            lcd.DisplayStringAt(20, 20, (uint8_t*)"2000", CENTER_MODE);
+        break;
+  }
+    read(MPU6050_GYRO_YOUT_H_REG ,&tmpreg , 1);
+    switch(tmpreg >> 6) {
+        case 0:
+            lcd.DisplayStringAt(20, 40, (uint8_t*)"", 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;
+    }
+
+    for (int i = 0; i < 50; i++) {
+        mpu.getGyro(GyroBuffer);
+        zero_level[0] += GyroBuffer[0] / 131;
+        zero_level[1] += GyroBuffer[1] / 131;
+        zero_level[2] += GyroBuffer[2] / 131;
+        
+        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++) {
+        mpu.getGyroRaw(GyroBuffer[0]);
+        if ((GyroBuffer[0] / 131 - zero_level[0]) > noise_level) {
+            noise_level = (GyroBuffer[0] / 131 - zero_level[0]);
+        } 
+        else if ((GyroBuffer[0] / 131 - zero_level[0]) < -noise_level) {
+            noise_level = -(GyroBuffer[0] / 131 - zero_level[0]);
+        }
+        wait_ms(10);
+    }
+    sprintf((char*)buffer, "Noise = %.2f\n", noise_level);
+    lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
+    sprintf((char*)buffer, "Zero = %.2f\n", zero_level[0]);
+    lcd.DisplayStringAt(20, 170, buffer, CENTER_MODE);
+    float angle = 0.0, prev_value = 0.0; 
+    bool status1 = false, status2 = false;
+    while(1) {
+        // Read Gyroscope values
+        wait(0.02);
+        
+        mpu.getGyroRaw(GyroBuffer);
+        float value = GyroBuffer[0] / 131 - zero_level[0];
+        if (value >= noise_level || value <= -noise_level) {
+            angle += (value + prev_value) / 2  * 0.02f ;
+        }
+        if (angle > 45.0){
+           status1 = true;
+            
+            
+        }else if  (angle < -45.0){            
+            status2 = true;
+            
+        } else if (status1 == true && status2 == true){
+            sprintf((char*)buffer, "Angle = %.3f\n", angle);
+            lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
+            lcd.DisplayStringAt(20, 120, (uint8_t*)"All Right", CENTER_MODE);
+            }
+        
+        prev_value = value;
+        sprintf((char*)buffer, "Angle = %.3f\n", angle);
+        lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
+        
+        
+    }
+}
\ No newline at end of file