Carlos Orendain
Self-Balancing bot for MAX32630 and two wheel car.
Dependencies: mbed BMI160 SDFileSystem max32630fthr
Diff: main.cpp
- Revision:
- 4:4144ef007743
- Parent:
- 3:7807a40b2459
- Child:
- 5:f66e4666de56
--- 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");
+ }
+}