Serhii Ovchar / Mbed 2 deprecated EmbeddedGyro

._.

Dependencies:   LCD_DISCO_F429ZI mbed BSP_DISCO_F429ZI GYRO_DISCO_F429ZI

main.cpp@0:3294aff3e5ad, 2021-12-15 (annotated)

Committer:
ovcharka132
Date:
Wed Dec 15 16:36:39 2021 +0000
Revision:
0:3294aff3e5ad
Lab 3 Embedded

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ovcharka132 0:3294aff3e5ad 1 #include "mbed.h"
ovcharka132 0:3294aff3e5ad 2 #include "GYRO_DISCO_F429ZI.h"
ovcharka132 0:3294aff3e5ad 3 #include "LCD_DISCO_F429ZI.h"
ovcharka132 0:3294aff3e5ad 4 #include "stm32f429i_discovery_lcd.h"
ovcharka132 0:3294aff3e5ad 5
ovcharka132 0:3294aff3e5ad 6 GYRO_DISCO_F429ZI gyro;
ovcharka132 0:3294aff3e5ad 7 LCD_DISCO_F429ZI lcd;
ovcharka132 0:3294aff3e5ad 8
ovcharka132 0:3294aff3e5ad 9 DigitalOut led1(LED1);
ovcharka132 0:3294aff3e5ad 10
ovcharka132 0:3294aff3e5ad 11 bool isInMotion = false;
ovcharka132 0:3294aff3e5ad 12
ovcharka132 0:3294aff3e5ad 13 int main()
ovcharka132 0:3294aff3e5ad 14 {
ovcharka132 0:3294aff3e5ad 15 float GyroBuffer[3];
ovcharka132 0:3294aff3e5ad 16 float zero_level[3] = {0, 0, 0};
ovcharka132 0:3294aff3e5ad 17 uint8_t buffer[100];
ovcharka132 0:3294aff3e5ad 18 lcd.Init();
ovcharka132 0:3294aff3e5ad 19 lcd.DisplayStringAt(0, LINE(1), (uint8_t*)"Started", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 20 uint8_t tmpreg = 0;
ovcharka132 0:3294aff3e5ad 21 GYRO_IO_Read(&tmpreg,I3G4250D_CTRL_REG4_ADDR,1);
ovcharka132 0:3294aff3e5ad 22 switch(tmpreg & I3G4250D_FULLSCALE_SELECTION)
ovcharka132 0:3294aff3e5ad 23 {
ovcharka132 0:3294aff3e5ad 24 case I3G4250D_FULLSCALE_245:
ovcharka132 0:3294aff3e5ad 25 lcd.DisplayStringAt(0, LINE(2), (uint8_t*)"245", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 26 break;
ovcharka132 0:3294aff3e5ad 27
ovcharka132 0:3294aff3e5ad 28 case I3G4250D_FULLSCALE_500:
ovcharka132 0:3294aff3e5ad 29 lcd.DisplayStringAt(0, LINE(2), (uint8_t*)"500", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 30 break;
ovcharka132 0:3294aff3e5ad 31
ovcharka132 0:3294aff3e5ad 32 case I3G4250D_FULLSCALE_2000:
ovcharka132 0:3294aff3e5ad 33 lcd.DisplayStringAt(0, LINE(2), (uint8_t*)"2000", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 34 break;
ovcharka132 0:3294aff3e5ad 35 }
ovcharka132 0:3294aff3e5ad 36 GYRO_IO_Read(&tmpreg, I3G4250D_CTRL_REG1_ADDR, 1);
ovcharka132 0:3294aff3e5ad 37 switch(tmpreg >> 6) {
ovcharka132 0:3294aff3e5ad 38 case 0:
ovcharka132 0:3294aff3e5ad 39 lcd.DisplayStringAt(0, LINE(3), (uint8_t*)"95", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 40 break;
ovcharka132 0:3294aff3e5ad 41 case 1:
ovcharka132 0:3294aff3e5ad 42 lcd.DisplayStringAt(0, LINE(3), (uint8_t*)"190", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 43 break;
ovcharka132 0:3294aff3e5ad 44 case 2:
ovcharka132 0:3294aff3e5ad 45 lcd.DisplayStringAt(0, LINE(3), (uint8_t*)"380", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 46 break;
ovcharka132 0:3294aff3e5ad 47 case 3:
ovcharka132 0:3294aff3e5ad 48 lcd.DisplayStringAt(0, LINE(3), (uint8_t*)"760", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 49 break;
ovcharka132 0:3294aff3e5ad 50 }
ovcharka132 0:3294aff3e5ad 51
ovcharka132 0:3294aff3e5ad 52 for (int i = 0; i < 50; i++) {
ovcharka132 0:3294aff3e5ad 53 gyro.GetXYZ(GyroBuffer);
ovcharka132 0:3294aff3e5ad 54 zero_level[0] += GyroBuffer[0] / 17.5f;
ovcharka132 0:3294aff3e5ad 55 zero_level[1] += GyroBuffer[1] / 17.5f;
ovcharka132 0:3294aff3e5ad 56 zero_level[2] += GyroBuffer[2] / 17.5f;
ovcharka132 0:3294aff3e5ad 57
ovcharka132 0:3294aff3e5ad 58 wait_ms(10);
ovcharka132 0:3294aff3e5ad 59 }
ovcharka132 0:3294aff3e5ad 60 for (int i = 0; i < 3; i++)
ovcharka132 0:3294aff3e5ad 61 zero_level[i] /= 50;
ovcharka132 0:3294aff3e5ad 62
ovcharka132 0:3294aff3e5ad 63 float noise_level = 0.0f;
ovcharka132 0:3294aff3e5ad 64 for (int i = 0; i < 50; i++) {
ovcharka132 0:3294aff3e5ad 65 gyro.GetXYZ(GyroBuffer);
ovcharka132 0:3294aff3e5ad 66 if ((GyroBuffer[0] / 17.5f - zero_level[0]) > noise_level) {
ovcharka132 0:3294aff3e5ad 67 noise_level = (GyroBuffer[1] / 17.5f - zero_level[0]);
ovcharka132 0:3294aff3e5ad 68 }
ovcharka132 0:3294aff3e5ad 69 else if ((GyroBuffer[0] / 17.5f - zero_level[0]) < -noise_level) {
ovcharka132 0:3294aff3e5ad 70 noise_level = -(GyroBuffer[1] / 17.5f - zero_level[0]);
ovcharka132 0:3294aff3e5ad 71 }
ovcharka132 0:3294aff3e5ad 72 wait_ms(10);
ovcharka132 0:3294aff3e5ad 73 }
ovcharka132 0:3294aff3e5ad 74 sprintf((char*)buffer, "Noise %.2f", noise_level);
ovcharka132 0:3294aff3e5ad 75 lcd.DisplayStringAt(0, LINE(4), buffer, CENTER_MODE);
ovcharka132 0:3294aff3e5ad 76 float angle = 0.0f, prev_value = 0.0f;
ovcharka132 0:3294aff3e5ad 77 float saved_angle = 0.0f;
ovcharka132 0:3294aff3e5ad 78 while(1) {
ovcharka132 0:3294aff3e5ad 79 // Read Gyroscope values
ovcharka132 0:3294aff3e5ad 80 wait(0.02);
ovcharka132 0:3294aff3e5ad 81
ovcharka132 0:3294aff3e5ad 82 gyro.GetXYZ(GyroBuffer);
ovcharka132 0:3294aff3e5ad 83 float value = GyroBuffer[0] / 17.5f - zero_level[0];
ovcharka132 0:3294aff3e5ad 84 if (value >= noise_level || value <= -noise_level) {
ovcharka132 0:3294aff3e5ad 85 if(value >= 0.1f) {
ovcharka132 0:3294aff3e5ad 86 if(!isInMotion){
ovcharka132 0:3294aff3e5ad 87 saved_angle = angle;
ovcharka132 0:3294aff3e5ad 88 }
ovcharka132 0:3294aff3e5ad 89 isInMotion = true;
ovcharka132 0:3294aff3e5ad 90 }
ovcharka132 0:3294aff3e5ad 91 angle += (value + prev_value) / 2 * 0.0175f * 0.02f / 0.825f;
ovcharka132 0:3294aff3e5ad 92 } else {
ovcharka132 0:3294aff3e5ad 93 isInMotion = false;
ovcharka132 0:3294aff3e5ad 94 if(fabs(saved_angle - angle) >= 45.0f){
ovcharka132 0:3294aff3e5ad 95 lcd.ClearStringLine(7);
ovcharka132 0:3294aff3e5ad 96 lcd.ClearStringLine(8);
ovcharka132 0:3294aff3e5ad 97 lcd.DisplayStringAt(0, LINE(7), (uint8_t*)"Gesture", CENTER_MODE);
ovcharka132 0:3294aff3e5ad 98 sprintf((char*)buffer, "Angle %.3f", angle - saved_angle);
ovcharka132 0:3294aff3e5ad 99 lcd.DisplayStringAt(0, LINE(8), buffer, CENTER_MODE);
ovcharka132 0:3294aff3e5ad 100 }
ovcharka132 0:3294aff3e5ad 101 saved_angle = angle;
ovcharka132 0:3294aff3e5ad 102 }
ovcharka132 0:3294aff3e5ad 103 prev_value = value;
ovcharka132 0:3294aff3e5ad 104 sprintf((char*)buffer, "Angle %.3f", angle);
ovcharka132 0:3294aff3e5ad 105 lcd.ClearStringLine(6);
ovcharka132 0:3294aff3e5ad 106 lcd.DisplayStringAt(0, LINE(6), buffer, CENTER_MODE);
ovcharka132 0:3294aff3e5ad 107
ovcharka132 0:3294aff3e5ad 108 }
ovcharka132 0:3294aff3e5ad 109 }