gyro
Dependencies: BSP_DISCO_F429ZI GYRO_DISCO_F429ZI LCD_DISCO_F429ZI mbed
Fork of DISCO-F429ZI_Gyro_demo by
main.cpp
- Committer:
- codebreaker7
- Date:
- 2017-11-15
- Revision:
- 3:68f55e42c85f
- Parent:
- 2:b878d61bf57b
File content as of revision 3:68f55e42c85f:
#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]; 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; } 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; 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.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); } }