Balancing Bot using MAX32630FTHR
Dependencies: BMI160 max32630fthr mbed SDFileSystem
Diff: main.cpp
- Revision:
- 4:4144ef007743
- Parent:
- 3:7807a40b2459
- Child:
- 5:f66e4666de56
diff -r 7807a40b2459 -r 4144ef007743 main.cpp --- a/main.cpp Mon Jan 09 19:08:30 2017 +0000 +++ b/main.cpp Thu Jan 12 22:11:45 2017 +0000 @@ -47,6 +47,10 @@ #include "bmi160.h" #include "SDFileSystem.h" + +void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, + float * accY, float * accZ, float * pw); + //Setup interrupt in from imu InterruptIn imuIntIn(P3_6); @@ -147,28 +151,12 @@ uint32_t failures = 0; DigitalIn uSDdetect(P2_2, PullUp); - SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs static const uint32_t N = 8000; uint32_t samples = 0; - float pitchBuff[N]; - float gyroBuff[N]; - float accBuff[N]; - FILE *fp; - //Erase old log and start a new one - if(!uSDdetect) - { - fp = fopen("/sd/balBot.txt", "w"); - if(fp != NULL) - { - fprintf(fp, "Balance Bot Data,\n"); - fclose(fp); - } - else - { - printf("Failed to open file\n"); - failures++; - } - } + float accYaxisBuff[N]; + float accZaxisBuff[N]; + float gyroXaxisBuff[N]; + float pulseWidthBuff[N]; //Setup I2C bus for IMU I2C i2cBus(P5_7, P6_0); @@ -285,17 +273,19 @@ BMI160::SensorTime sensorTime; //Complementary Filter vars, filter weight K - static const float K = 0.9993F; - float thetaGyro = 0.0F; + static const float K = 0.9F; float thetaAcc = 0.0F; float pitch = 0.0F; //PID coefficients + //Kc = 2.5, Pc = 0.1; Critical gain and period of osc + //when adjusting KP until marginally stable static const float DT = 0.000625F; - static const float KP = 4.25F; - static const float KI = (8.5F * DT); - static const float KD = (0.0425F / DT); + static const float KP = 2.0F; + static const float KI = (0.0F*DT); + static const float KD = (0.0F/DT); float setPoint = 0.0F; + float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT}; //Control loop vars float currentError, previousError; @@ -332,6 +322,7 @@ //Position control vars/constants static const float PING_SP = 20.0F; + static const float PING_KP = 0.0F; float pingCurrentError = 0.0F; //control loop timming indicator @@ -363,13 +354,9 @@ //Read feedback sensors imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); - //Feedback Block, Complementary filter - //Prediction estimate - thetaGyro = (pitch + (gyroData.xAxis.scaled * DT)); - //Measurement estimate - thetaAcc = ((180.0F/3.14F) * atan(accData.yAxis.scaled/accData.zAxis.scaled)); - //Feedback, Pitch estimate in degrees - pitch = ((K * thetaGyro) + ((1.0F - K) * thetaAcc)); + //Feedback Block, Complementary filter. + //Pitch estimate in degrees + pitch = ((K * (pitch + (gyroData.xAxis.scaled * DT))) + ((1.0F - K) * accData.yAxis.scaled)); //Accumalate pitch measurements setPoint += pitch; @@ -402,21 +389,9 @@ //Read feedback sensors imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); - //Feedback Block, Complementary filter - //Prediction estimate - thetaGyro = (pitch + (gyroData.xAxis.scaled * DT)); - //Measurement estimate - thetaAcc = ((180.0F/3.14F) * atan(accData.yAxis.scaled/accData.zAxis.scaled)); - //Feedback, Pitch estimate in degrees - pitch = ((K * thetaGyro) + ((1.0F - K) * thetaAcc)); - - if(samples < N) - { - pitchBuff[samples] = pitch; - gyroBuff[samples] = thetaGyro; - accBuff[samples] = thetaAcc; - samples++; - } + //Feedback Block, Complementary filter. + //Pitch estimate in degrees + pitch = ((K * (pitch + (gyroData.xAxis.scaled * DT))) + ((1.0F - K) * accData.yAxis.scaled)); //PID Block //Error signal @@ -454,7 +429,7 @@ pingCurrentError = PING_SP - cm; } - //pulseWidth += pingCurrentError * -1.0F; + pulseWidth += (pingCurrentError * PING_KP); //Clamp to maximum duty cycle and check for forward/reverse @@ -463,6 +438,7 @@ { rightDir = FORWARD; leftDir = FORWARD; + pulseWidth = 40.0F; rightPwm.pulsewidth_us(40); leftPwm.pulsewidth_us(40); } @@ -470,6 +446,7 @@ { rightDir = REVERSE; leftDir = REVERSE; + pulseWidth = -40.0F; rightPwm.pulsewidth_us(40); leftPwm.pulsewidth_us(40); } @@ -479,9 +456,8 @@ { rightDir = REVERSE; leftDir = REVERSE; - pulseWidth = (1 - pulseWidth); - rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); - leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); + rightPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth)); + leftPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth)); } else { @@ -492,14 +468,30 @@ } } + if(samples < N) + { + accYaxisBuff[samples] = accData.yAxis.scaled; + accZaxisBuff[samples] = accData.zAxis.scaled; + gyroXaxisBuff[samples] = gyroData.xAxis.scaled; + pulseWidthBuff[samples] = pulseWidth; + samples++; + } + //save current error for next loop previousError = currentError; - + //Stop loopPulse = !loopPulse; } } + if((pitch > 30.0F) || (pitch < -30.0F)) + { + start = false; + } + pitch = 0.0F; + integral = 0.0F; + previousError = 0.0F; rightPwm.pulsewidth_us(0); leftPwm.pulsewidth_us(0); @@ -511,36 +503,12 @@ wait(0.25); if(!uSDdetect && (samples > 0)) - { + { + loopCoeffs[0] = setPoint; + bLED = LED_ON; + saveData(samples, loopCoeffs, gyroXaxisBuff, accYaxisBuff, accZaxisBuff, pulseWidthBuff); samples = 0; - fp = fopen("/sd/balBot.txt", "a"); - if(fp != NULL) - { - bLED = LED_ON; - fprintf(fp, "KI and KD have DT rolled into them; ie * or / respectively.\n"); - fprintf(fp, "Setpoint,%5.2f,\n", setPoint); - fprintf(fp, "K, %f,\n", K); - fprintf(fp, "KP, %f,\n", KP); - fprintf(fp, "KI, %f,\n", KI); - fprintf(fp, "KD, %f,\n", KD); - fprintf(fp, "DT, %f,\n", DT); - fprintf(fp, "Time, Pitch, Gyro, Acc,\n"); - for(uint32_t idx = 0; idx < N; idx++) - { - fprintf(fp, "%f,", idx*DT); - fprintf(fp, "%5.2f,", pitchBuff[idx]); - fprintf(fp, "%5.2f,", gyroBuff[idx]); - fprintf(fp, "%5.2f,", accBuff[idx]); - fprintf(fp, "\n"); - } - fprintf(fp, "\n"); - fclose(fp); - bLED = LED_OFF; - } - else - { - printf("Failed to open file\n"); - } + bLED = LED_OFF; } } } @@ -553,3 +521,52 @@ } } } + + +//***************************************************************************** +void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, + float * accY, float * accZ, float * pw) +{ + SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs + FILE *fp; + + fp = fopen("/sd/balBot.txt", "a"); + if(fp != NULL) + { + fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]); + fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]); + fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]); + fprintf(fp, "KI, %f,,,,,\n", loopCoeffs[3]); + fprintf(fp, "KD, %f,,,,,\n", loopCoeffs[4]); + fprintf(fp, "DT, %f,,,,,\n", loopCoeffs[5]); + fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n"); + + float accPitch = 0.0F; + float gyroPitch = 0.0F; + float pitch = 0.0F; + float k = loopCoeffs[1]; + float dt = loopCoeffs[5]; + + for(uint32_t idx = 0; idx < numSamples; idx++) + { + //Time + fprintf(fp, "%f,", idx*loopCoeffs[5]); + fprintf(fp, "%5.4f,", accY[idx]); + fprintf(fp, "%6.2f,", gyroX[idx]); + gyroPitch += (gyroX[idx]*loopCoeffs[5]); + fprintf(fp, "%5.2f,", gyroPitch); + accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx])); + fprintf(fp, "%5.2f,", accPitch); + pitch = ((k * (pitch + (gyroX[idx] * dt))) + ((1.0F - k) * accY[idx])); + fprintf(fp, "%5.2f", pitch); + fprintf(fp, "%f", pw[idx]); + fprintf(fp, "\n"); + } + fprintf(fp, "\n"); + fclose(fp); + } + else + { + printf("Failed to open file\n"); + } +}