gyro
Dependencies: BSP_DISCO_F429ZI GYRO_DISCO_F429ZI LCD_DISCO_F429ZI mbed
Fork of DISCO-F429ZI_Gyro_demo by
main.cpp@2:b878d61bf57b, 2017-02-16 (annotated)
- Committer:
- codebreaker7
- Date:
- Thu Feb 16 11:43:51 2017 +0000
- Revision:
- 2:b878d61bf57b
- Parent:
- 0:44f624c5501e
- Child:
- 3:68f55e42c85f
Gyro example
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bcostm | 0:44f624c5501e | 1 | #include "mbed.h" |
bcostm | 0:44f624c5501e | 2 | #include "GYRO_DISCO_F429ZI.h" |
codebreaker7 | 2:b878d61bf57b | 3 | #include "LCD_DISCO_F429ZI.h" |
codebreaker7 | 2:b878d61bf57b | 4 | #include "stm32f429i_discovery_lcd.h" |
bcostm | 0:44f624c5501e | 5 | |
bcostm | 0:44f624c5501e | 6 | GYRO_DISCO_F429ZI gyro; |
codebreaker7 | 2:b878d61bf57b | 7 | LCD_DISCO_F429ZI lcd; |
bcostm | 0:44f624c5501e | 8 | |
bcostm | 0:44f624c5501e | 9 | DigitalOut led1(LED1); |
bcostm | 0:44f624c5501e | 10 | |
bcostm | 0:44f624c5501e | 11 | int main() |
bcostm | 0:44f624c5501e | 12 | { |
bcostm | 0:44f624c5501e | 13 | float GyroBuffer[3]; |
codebreaker7 | 2:b878d61bf57b | 14 | float zero_level[3] = {0, 0, 0}; |
codebreaker7 | 2:b878d61bf57b | 15 | uint8_t buffer[100]; |
codebreaker7 | 2:b878d61bf57b | 16 | lcd.Init(); |
codebreaker7 | 2:b878d61bf57b | 17 | lcd.DisplayStringAt(10, 10, (uint8_t*)"Gyroscope_started", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 18 | uint8_t tmpreg = 0; |
codebreaker7 | 2:b878d61bf57b | 19 | GYRO_IO_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1); |
codebreaker7 | 2:b878d61bf57b | 20 | switch(tmpreg & L3GD20_FULLSCALE_SELECTION) |
codebreaker7 | 2:b878d61bf57b | 21 | { |
codebreaker7 | 2:b878d61bf57b | 22 | case L3GD20_FULLSCALE_250: |
codebreaker7 | 2:b878d61bf57b | 23 | lcd.DisplayStringAt(20, 20, (uint8_t*)"250", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 24 | break; |
codebreaker7 | 2:b878d61bf57b | 25 | |
codebreaker7 | 2:b878d61bf57b | 26 | case L3GD20_FULLSCALE_500: |
codebreaker7 | 2:b878d61bf57b | 27 | lcd.DisplayStringAt(20, 20, (uint8_t*)"500", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 28 | break; |
codebreaker7 | 2:b878d61bf57b | 29 | |
codebreaker7 | 2:b878d61bf57b | 30 | case L3GD20_FULLSCALE_2000: |
codebreaker7 | 2:b878d61bf57b | 31 | lcd.DisplayStringAt(20, 20, (uint8_t*)"2000", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 32 | break; |
codebreaker7 | 2:b878d61bf57b | 33 | } |
codebreaker7 | 2:b878d61bf57b | 34 | GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG1_ADDR, 1); |
codebreaker7 | 2:b878d61bf57b | 35 | switch(tmpreg >> 6) { |
codebreaker7 | 2:b878d61bf57b | 36 | case 0: |
codebreaker7 | 2:b878d61bf57b | 37 | lcd.DisplayStringAt(20, 40, (uint8_t*)"95", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 38 | break; |
codebreaker7 | 2:b878d61bf57b | 39 | case 1: |
codebreaker7 | 2:b878d61bf57b | 40 | lcd.DisplayStringAt(20, 40, (uint8_t*)"190", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 41 | break; |
codebreaker7 | 2:b878d61bf57b | 42 | case 2: |
codebreaker7 | 2:b878d61bf57b | 43 | lcd.DisplayStringAt(20, 40, (uint8_t*)"380", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 44 | break; |
codebreaker7 | 2:b878d61bf57b | 45 | case 3: |
codebreaker7 | 2:b878d61bf57b | 46 | lcd.DisplayStringAt(20, 40, (uint8_t*)"760", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 47 | break; |
codebreaker7 | 2:b878d61bf57b | 48 | } |
codebreaker7 | 2:b878d61bf57b | 49 | //BSP_LCD_Init(); |
codebreaker7 | 2:b878d61bf57b | 50 | //BSP_LCD_De |
codebreaker7 | 2:b878d61bf57b | 51 | //BSP_LCD_DisplayStringAt(10,10, (uint8_t*)"Gyroscope_started", CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 52 | |
codebreaker7 | 2:b878d61bf57b | 53 | for (int i = 0; i < 50; i++) { |
codebreaker7 | 2:b878d61bf57b | 54 | gyro.GetXYZ(GyroBuffer); |
codebreaker7 | 2:b878d61bf57b | 55 | zero_level[0] += GyroBuffer[0] / 17.5f; |
codebreaker7 | 2:b878d61bf57b | 56 | zero_level[1] += GyroBuffer[1] / 17.5f; |
codebreaker7 | 2:b878d61bf57b | 57 | zero_level[2] += GyroBuffer[2] / 17.5f; |
codebreaker7 | 2:b878d61bf57b | 58 | //zero_level[0] += GyroBuffer[0]; |
codebreaker7 | 2:b878d61bf57b | 59 | //zero_level[1] += GyroBuffer[1]; |
codebreaker7 | 2:b878d61bf57b | 60 | //zero_level[2] += GyroBuffer[2]; |
codebreaker7 | 2:b878d61bf57b | 61 | wait_ms(10); |
codebreaker7 | 2:b878d61bf57b | 62 | } |
codebreaker7 | 2:b878d61bf57b | 63 | for (int i = 0; i < 3; i++) |
codebreaker7 | 2:b878d61bf57b | 64 | zero_level[i] /= 50; |
codebreaker7 | 2:b878d61bf57b | 65 | |
codebreaker7 | 2:b878d61bf57b | 66 | float noise_level = 0.0; |
codebreaker7 | 2:b878d61bf57b | 67 | for (int i = 0; i < 50; i++) { |
codebreaker7 | 2:b878d61bf57b | 68 | gyro.GetXYZ(GyroBuffer); |
codebreaker7 | 2:b878d61bf57b | 69 | if ((GyroBuffer[0] / 17.5f - zero_level[0]) > noise_level) { |
codebreaker7 | 2:b878d61bf57b | 70 | noise_level = (GyroBuffer[0] / 17.5f - zero_level[0]); |
codebreaker7 | 2:b878d61bf57b | 71 | } |
codebreaker7 | 2:b878d61bf57b | 72 | else if ((GyroBuffer[0] / 17.5f - zero_level[0]) < -noise_level) { |
codebreaker7 | 2:b878d61bf57b | 73 | noise_level = -(GyroBuffer[0] / 17.5f - zero_level[0]); |
codebreaker7 | 2:b878d61bf57b | 74 | } |
codebreaker7 | 2:b878d61bf57b | 75 | wait_ms(10); |
codebreaker7 | 2:b878d61bf57b | 76 | } |
codebreaker7 | 2:b878d61bf57b | 77 | sprintf((char*)buffer, "Noise = %.2f\n", noise_level); |
codebreaker7 | 2:b878d61bf57b | 78 | lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 79 | float angle = 0.0, prev_value = 0.0; |
bcostm | 0:44f624c5501e | 80 | while(1) { |
bcostm | 0:44f624c5501e | 81 | // Read Gyroscope values |
codebreaker7 | 2:b878d61bf57b | 82 | wait(0.02); |
codebreaker7 | 2:b878d61bf57b | 83 | //GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1); |
codebreaker7 | 2:b878d61bf57b | 84 | //while((tmpreg >> 3) & 0x01 != 1) GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1); |
bcostm | 0:44f624c5501e | 85 | gyro.GetXYZ(GyroBuffer); |
codebreaker7 | 2:b878d61bf57b | 86 | float value = GyroBuffer[0] / 17.5f - zero_level[0]; |
codebreaker7 | 2:b878d61bf57b | 87 | if (value >= noise_level || value <= -noise_level) { |
codebreaker7 | 2:b878d61bf57b | 88 | angle += (value + prev_value) / 2 * 0.0175 * 0.02f / 0.825; |
codebreaker7 | 2:b878d61bf57b | 89 | } |
codebreaker7 | 2:b878d61bf57b | 90 | prev_value = value; |
codebreaker7 | 2:b878d61bf57b | 91 | sprintf((char*)buffer, "Angle = %.3f\n", angle); |
codebreaker7 | 2:b878d61bf57b | 92 | lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE); |
bcostm | 0:44f624c5501e | 93 | // Display values |
codebreaker7 | 2:b878d61bf57b | 94 | //sprintf((char*)buffer, "X = %.2f\n", (GyroBuffer[0] - zero_level[0]) / 1000); |
codebreaker7 | 2:b878d61bf57b | 95 | //sprintf((char*)buffer, "X = %.2f\n", GyroBuffer[0] / 17.5f - zero_level[0]); |
codebreaker7 | 2:b878d61bf57b | 96 | //lcd.DisplayStringAt(20, 10, buffer, CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 97 | //sprintf((char*)buffer, "Y = %.2f\n", (GyroBuffer[1] - zero_level[1]) / 1000); |
codebreaker7 | 2:b878d61bf57b | 98 | //sprintf((char*)buffer, "Y = %.2f\n", GyroBuffer[1] / 17.5f - zero_level[1]); |
codebreaker7 | 2:b878d61bf57b | 99 | //lcd.DisplayStringAt(20, 40, buffer, CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 100 | //sprintf((char*)buffer, "Z = %.2f\n", (GyroBuffer[2] - zero_level[2]) / 1000); |
codebreaker7 | 2:b878d61bf57b | 101 | //sprintf((char*)buffer, "Z = %.2f\n", GyroBuffer[2] / 17.5f - zero_level[2]); |
codebreaker7 | 2:b878d61bf57b | 102 | //lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE); |
codebreaker7 | 2:b878d61bf57b | 103 | //led1 = !led1; |
codebreaker7 | 2:b878d61bf57b | 104 | //wait(1); |
codebreaker7 | 2:b878d61bf57b | 105 | //lcd.Clear(0xffffffff); |
bcostm | 0:44f624c5501e | 106 | } |
bcostm | 0:44f624c5501e | 107 | } |