Self-Balancing bot for MAX32630 and two wheel car.
Dependencies: mbed BMI160 SDFileSystem max32630fthr
Diff: main.cpp
- Revision:
- 8:810396d34da8
- Parent:
- 7:337faf16a8d6
- Child:
- 9:59add4a4372c
--- a/main.cpp Mon Jan 16 05:31:33 2017 +0000 +++ b/main.cpp Wed Jan 25 19:33:27 2017 +0000 @@ -278,15 +278,15 @@ BMI160::SensorTime sensorTime; //Complementary Filter vars, filter weight K - static const float K = 0.999F; + static const float K = 0.9965F; float pitch = 0.0F; //PID coefficients static const float DT = 0.00125F; - static const float KP = 3.0F; - static const float KI = (30.0F*DT); - static const float KD = (0.06F/DT); + static const float KP = 1.85F; + static const float KI = (20.0F*DT); + static const float KD = (0.050F/DT); float setPoint = 0.0F; float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT}; @@ -374,7 +374,7 @@ } - while(start && (pitch > -30.0F) && (pitch < 30.0F)) + while(start && (pitch > -20.0F) && (pitch < 20.0F)) { rLED = LED_OFF; gLED = LED_ON; @@ -486,10 +486,11 @@ } } - if((pitch > 30.0F) || (pitch < -30.0F)) + if((pitch > 20.0F) || (pitch < -20.0F)) { start = false; } + pitch = 0.0F; integral = 0.0F; previousError = 0.0F; @@ -534,6 +535,7 @@ fp = fopen("/sd/balBot.txt", "a"); if(fp != NULL) { + fprintf(fp, "Samples,%d,,,,,\n", numSamples); fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]); fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]); fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]); @@ -576,5 +578,4 @@ 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)))); - //return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * accY)); }