Self-Balancing bot for MAX32630 and two wheel car.
Dependencies: mbed BMI160 SDFileSystem max32630fthr
Diff: main.cpp
- Revision:
- 12:0fb3a03aeecb
- Parent:
- 11:371026db1dcd
- Child:
- 14:988b8d294259
diff -r 371026db1dcd -r 0fb3a03aeecb main.cpp --- a/main.cpp Sat Jan 28 05:10:07 2017 +0000 +++ b/main.cpp Mon Feb 06 20:34:55 2017 +0000 @@ -170,6 +170,22 @@ //Get IMU instance and configure it BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); + //Power up sensors in normal mode + if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + printf("Failed to set gyroscope power mode\n"); + failures++; + } + + wait(0.1); + + if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + printf("Failed to set accelerometer power mode\n"); + failures++; + } + wait(0.1); + BMI160::AccConfig accConfig; BMI160::AccConfig accConfigRead; accConfig.range = BMI160::SENS_4G; @@ -246,20 +262,6 @@ failures++; } - //Power up sensors in normal mode - if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) - { - printf("Failed to set gyroscope power mode\n"); - failures++; - } - - wait(0.1); - - if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) - { - printf("Failed to set accelerometer power mode\n"); - failures++; - } if(failures == 0) { @@ -400,6 +402,7 @@ //Error signal currentError = (setPoint - pitch); + //Integral term, dt is included in KI integral = (integral + currentError); //Derivative term, dt is included in KD