![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
New Self Balancing Code
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
Diff: main.cpp
- Revision:
- 5:f6727800e43f
- Parent:
- 4:fc8ef84a7dbc
- Child:
- 7:2c48702d99e3
--- a/main.cpp Wed Dec 05 04:44:17 2018 +0000 +++ b/main.cpp Thu Dec 13 11:17:03 2018 +0000 @@ -37,6 +37,7 @@ #include "max32630fthr.h" #include "USBSerial.h" #include "stdlib.h" +#include "math.h" //MAX32630HSP icarus(MAX32630HSP::VIO_3V3); MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); @@ -64,6 +65,14 @@ void printRegister(BMI160 &imu, BMI160::Registers reg); void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg); void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data); +//float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT); + + +float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT) +{ + return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ)))); +} + int main() { @@ -142,6 +151,8 @@ pc.printf("GYRO Range = %d\r\n", gyroConfig.range); pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp); pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr); + + } else { @@ -177,15 +188,28 @@ BMI160::SensorTime sensorTime; //PwmPin = 1; - + float apitch = 0; + float k = 0.6; + + time_1 = sensorTime.seconds; while(1) { imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); imu.getTemperature(&imuTemperature); + time_2 = sensorTime.seconds; + apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1); + daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch); + time_1 = time_2; + //printRegister(imu, BMI160::GYR_CONF); + } + } +} + +/* if(timeFlag == true){ currentGyroX = gyroData.xAxis.scaled; @@ -214,12 +238,12 @@ { M_1 = 0; M_2 = 1; - pc.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation); + daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation); } else{ M_1 = 1; M_2 = 0; - pc.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation); + daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation); } //------------------------ @@ -251,7 +275,9 @@ wait(0.6); } } -} +*/ + + //*****************************************************************************