Nikita Ivanykov / Mbed 2 deprecated embed_6

5

Dependencies:   mbed LCD_DISCO_F746NG MPU6050 BSP_DISCO_F746NG

main.cpp@0:a3739a2e895d, 2019-11-13 (annotated)

Committer:
troshha
Date:
Wed Nov 13 10:28:00 2019 +0000
Revision:
0:a3739a2e895d
5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
troshha 0:a3739a2e895d 1 #include "mbed.h"
troshha 0:a3739a2e895d 2 #include "MPU6050.h"
troshha 0:a3739a2e895d 3 #include "LCD_DISCO_F746NG.h"
troshha 0:a3739a2e895d 4 #include "stm32746g_discovery_lcd.h"
troshha 0:a3739a2e895d 5
troshha 0:a3739a2e895d 6 LCD_DISCO_F746NG lcd;
troshha 0:a3739a2e895d 7 DigitalOut myled(LED1);
troshha 0:a3739a2e895d 8 MPU6050 mpu(D14, D15);
troshha 0:a3739a2e895d 9
troshha 0:a3739a2e895d 10 int main()
troshha 0:a3739a2e895d 11 {
troshha 0:a3739a2e895d 12 float GyroBuffer[3];
troshha 0:a3739a2e895d 13 float zero_level[3] = {0, 0, 0};
troshha 0:a3739a2e895d 14 uint8_t buffer[100];
troshha 0:a3739a2e895d 15 lcd.Init();
troshha 0:a3739a2e895d 16 lcd.DisplayStringAt(10, 10, (uint8_t*)"Gyros_star", CENTER_MODE);
troshha 0:a3739a2e895d 17 uint8_t tmpreg = 0;
troshha 0:a3739a2e895d 18 read(MPU6050_GYRO_CONFIG_REG,&tmpreg,1);
troshha 0:a3739a2e895d 19 switch(tmpreg & MPU6050_GYRO_XOUT_H_REG)
troshha 0:a3739a2e895d 20 {
troshha 0:a3739a2e895d 21 case MPU6050_GYRO_RANGE_250:
troshha 0:a3739a2e895d 22 lcd.DisplayStringAt(20, 20, (uint8_t*)"250", CENTER_MODE);
troshha 0:a3739a2e895d 23 break;
troshha 0:a3739a2e895d 24
troshha 0:a3739a2e895d 25 case MPU6050_GYRO_RANGE_500:
troshha 0:a3739a2e895d 26 lcd.DisplayStringAt(20, 20, (uint8_t*)"500", CENTER_MODE);
troshha 0:a3739a2e895d 27 break;
troshha 0:a3739a2e895d 28
troshha 0:a3739a2e895d 29 case MPU6050_GYRO_RANGE_2000:
troshha 0:a3739a2e895d 30 lcd.DisplayStringAt(20, 20, (uint8_t*)"2000", CENTER_MODE);
troshha 0:a3739a2e895d 31 break;
troshha 0:a3739a2e895d 32 }
troshha 0:a3739a2e895d 33 read(MPU6050_GYRO_YOUT_H_REG ,&tmpreg , 1);
troshha 0:a3739a2e895d 34 switch(tmpreg >> 6) {
troshha 0:a3739a2e895d 35 case 0:
troshha 0:a3739a2e895d 36 lcd.DisplayStringAt(20, 40, (uint8_t*)"", CENTER_MODE);
troshha 0:a3739a2e895d 37 break;
troshha 0:a3739a2e895d 38 case 1:
troshha 0:a3739a2e895d 39 lcd.DisplayStringAt(20, 40, (uint8_t*)"190", CENTER_MODE);
troshha 0:a3739a2e895d 40 break;
troshha 0:a3739a2e895d 41 case 2:
troshha 0:a3739a2e895d 42 lcd.DisplayStringAt(20, 40, (uint8_t*)"380", CENTER_MODE);
troshha 0:a3739a2e895d 43 break;
troshha 0:a3739a2e895d 44 case 3:
troshha 0:a3739a2e895d 45 lcd.DisplayStringAt(20, 40, (uint8_t*)"760", CENTER_MODE);
troshha 0:a3739a2e895d 46 break;
troshha 0:a3739a2e895d 47 }
troshha 0:a3739a2e895d 48
troshha 0:a3739a2e895d 49 for (int i = 0; i < 50; i++) {
troshha 0:a3739a2e895d 50 mpu.getGyro(GyroBuffer);
troshha 0:a3739a2e895d 51 zero_level[0] += GyroBuffer[0] / 131;
troshha 0:a3739a2e895d 52 zero_level[1] += GyroBuffer[1] / 131;
troshha 0:a3739a2e895d 53 zero_level[2] += GyroBuffer[2] / 131;
troshha 0:a3739a2e895d 54
troshha 0:a3739a2e895d 55 wait_ms(10);
troshha 0:a3739a2e895d 56 }
troshha 0:a3739a2e895d 57 for (int i = 0; i < 3; i++)
troshha 0:a3739a2e895d 58 zero_level[i] /= 50;
troshha 0:a3739a2e895d 59
troshha 0:a3739a2e895d 60
troshha 0:a3739a2e895d 61 float noise_level = 0.0;
troshha 0:a3739a2e895d 62 for (int i = 0; i < 50; i++) {
troshha 0:a3739a2e895d 63 mpu.getGyroRaw(GyroBuffer[0]);
troshha 0:a3739a2e895d 64 if ((GyroBuffer[0] / 131 - zero_level[0]) > noise_level) {
troshha 0:a3739a2e895d 65 noise_level = (GyroBuffer[0] / 131 - zero_level[0]);
troshha 0:a3739a2e895d 66 }
troshha 0:a3739a2e895d 67 else if ((GyroBuffer[0] / 131 - zero_level[0]) < -noise_level) {
troshha 0:a3739a2e895d 68 noise_level = -(GyroBuffer[0] / 131 - zero_level[0]);
troshha 0:a3739a2e895d 69 }
troshha 0:a3739a2e895d 70 wait_ms(10);
troshha 0:a3739a2e895d 71 }
troshha 0:a3739a2e895d 72 sprintf((char*)buffer, "Noise = %.2f\n", noise_level);
troshha 0:a3739a2e895d 73 lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
troshha 0:a3739a2e895d 74 sprintf((char*)buffer, "Zero = %.2f\n", zero_level[0]);
troshha 0:a3739a2e895d 75 lcd.DisplayStringAt(20, 170, buffer, CENTER_MODE);
troshha 0:a3739a2e895d 76 float angle = 0.0, prev_value = 0.0;
troshha 0:a3739a2e895d 77 bool status1 = false, status2 = false;
troshha 0:a3739a2e895d 78 while(1) {
troshha 0:a3739a2e895d 79 // Read Gyroscope values
troshha 0:a3739a2e895d 80 wait(0.02);
troshha 0:a3739a2e895d 81
troshha 0:a3739a2e895d 82 mpu.getGyroRaw(GyroBuffer);
troshha 0:a3739a2e895d 83 float value = GyroBuffer[0] / 131 - zero_level[0];
troshha 0:a3739a2e895d 84 if (value >= noise_level || value <= -noise_level) {
troshha 0:a3739a2e895d 85 angle += (value + prev_value) / 2 * 0.02f ;
troshha 0:a3739a2e895d 86 }
troshha 0:a3739a2e895d 87 if (angle > 45.0){
troshha 0:a3739a2e895d 88 status1 = true;
troshha 0:a3739a2e895d 89
troshha 0:a3739a2e895d 90
troshha 0:a3739a2e895d 91 }else if (angle < -45.0){
troshha 0:a3739a2e895d 92 status2 = true;
troshha 0:a3739a2e895d 93
troshha 0:a3739a2e895d 94 } else if (status1 == true && status2 == true){
troshha 0:a3739a2e895d 95 sprintf((char*)buffer, "Angle = %.3f\n", angle);
troshha 0:a3739a2e895d 96 lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
troshha 0:a3739a2e895d 97 lcd.DisplayStringAt(20, 120, (uint8_t*)"All Right", CENTER_MODE);
troshha 0:a3739a2e895d 98 }
troshha 0:a3739a2e895d 99
troshha 0:a3739a2e895d 100 prev_value = value;
troshha 0:a3739a2e895d 101 sprintf((char*)buffer, "Angle = %.3f\n", angle);
troshha 0:a3739a2e895d 102 lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
troshha 0:a3739a2e895d 103
troshha 0:a3739a2e895d 104
troshha 0:a3739a2e895d 105 }
troshha 0:a3739a2e895d 106 }