Justin Jordan
Balancing Bot using MAX32630FTHR
Dependencies: BMI160 max32630fthr mbed SDFileSystem
Diff: main.cpp
- Revision:
- 3:7807a40b2459
- Parent:
- 2:5af1d818331f
- Child:
- 4:4144ef007743
--- a/main.cpp Fri Jan 06 20:32:13 2017 +0000
+++ b/main.cpp Mon Jan 09 19:08:30 2017 +0000
@@ -45,6 +45,7 @@
#include "mbed.h"
#include "max32630fthr.h"
#include "bmi160.h"
+#include "SDFileSystem.h"
//Setup interrupt in from imu
@@ -62,14 +63,52 @@
bool start = false;
void startStopISR()
{
- if(start)
- {
- start = false;
- }
- else
- {
- start = true;
- }
+ start = !start;
+}
+
+
+//Setup callibrate button
+DigitalIn callibrateBtn(P5_2, PullUp);
+InterruptIn callibrate(P5_2);
+bool cal = false;
+void callibrateISR()
+{
+ cal = !cal;
+}
+
+
+//Ping sensor trigger
+DigitalOut pingTrigger(P3_2, 0);
+Ticker pingTriggerTimer;
+
+void triggerPing()
+{
+ pingTrigger = !pingTrigger;
+ wait_us(1);
+ pingTrigger = !pingTrigger;
+}
+
+DigitalIn p51(P5_1, PullNone);
+InterruptIn pingEchoRiseInt(P5_1);
+DigitalIn p45(P4_5, PullNone);
+InterruptIn pingEchoFallInt(P4_5);
+Timer pingTimer;
+
+float cm = 0;
+bool pingReady = false;
+const float US_PER_CM = 29.387;
+
+void echoStartISR()
+{
+ pingTimer.reset();
+}
+
+void echoStopISR()
+{
+ uint32_t us = pingTimer.read_us()/2;
+
+ cm = (us/ US_PER_CM);
+ pingReady = true;
}
@@ -105,6 +144,32 @@
DigitalOut gLED(LED2, LED_OFF);
DigitalOut bLED(LED3, LED_OFF);
+ 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++;
+ }
+ }
+
//Setup I2C bus for IMU
I2C i2cBus(P5_7, P6_0);
i2cBus.frequency(400000);
@@ -112,8 +177,6 @@
//Get IMU instance and configure it
BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
- uint32_t failures = 0;
-
BMI160::AccConfig accConfig;
BMI160::AccConfig accConfigRead;
accConfig.range = BMI160::SENS_2G;
@@ -230,8 +293,8 @@
//PID coefficients
static const float DT = 0.000625F;
static const float KP = 4.25F;
- static const float KI = (8.0F * DT);
- static const float KD = (0.04F / DT);
+ static const float KI = (8.5F * DT);
+ static const float KD = (0.0425F / DT);
float setPoint = 0.0F;
//Control loop vars
@@ -246,12 +309,31 @@
imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A);
//Map data ready interrupt to INT1
imu.writeRegister(BMI160::INT_MAP_1, 0x80);
+
//Tie INT1 to callback fx
imuIntIn.rise(&imuISR);
//Tie SW2 to callback fx
startStop.fall(&startStopISR);
+ //Tie P5_2 to callback fx
+ callibrate.fall(&callibrateISR);
+ bool firstCal = true;
+
+ //Callbacks for echo measurement
+ pingEchoRiseInt.fall(&echoStartISR);
+ pingEchoFallInt.rise(&echoStopISR);
+
+ //Timer for echo measurements
+ pingTimer.start();
+
+ //Timer for trigger
+ pingTriggerTimer.attach(&triggerPing, 0.05);
+
+ //Position control vars/constants
+ static const float PING_SP = 20.0F;
+ float pingCurrentError = 0.0F;
+
//control loop timming indicator
DigitalOut loopPulse(P5_3, 0);
@@ -260,61 +342,54 @@
while(1)
{
- rLED = LED_ON;
- gLED = LED_ON;
- pitch = 0.0F;
- setPoint = 0.0F;
- rightPwm.pulsewidth_us(0);
- leftPwm.pulsewidth_us(0);
- start = false;
- drdy = false;
- while(offsetCount < 3200)
+ if(cal || (firstCal == true))
{
- if(drdy)
+ cal = false;
+ firstCal = false;
+ pitch = 0.0F;
+ setPoint = 0.0F;
+
+ rLED = LED_ON;
+ gLED = LED_ON;
+
+ while(offsetCount < 3200)
{
- //Clear data ready flag
- drdy = false;
-
- //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));
-
- //Accumalate pitch measurements
- setPoint += pitch;
- offsetCount++;
+ if(drdy)
+ {
+ //Clear data ready flag
+ drdy = false;
+
+ //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));
+
+ //Accumalate pitch measurements
+ setPoint += pitch;
+ offsetCount++;
+ }
}
+
+ //Average measurements
+ setPoint = setPoint/3200.0F;
+ printf("setPoint = %5.2f\n\n", setPoint);
+ //Clear count for next time
+ offsetCount = 0;
}
- //Clear count for next time
- offsetCount = 0;
-
- //Average measurements
- setPoint = setPoint/3200.0F;
-
- printf("setPoint = %5.2f\n\n", setPoint);
-
- while(!start)
- {
- rLED = LED_ON;
- gLED = LED_ON;
- wait(0.25);
- rLED = LED_OFF;
- gLED = LED_OFF;
- wait(0.25);
- }
while(start && (pitch > -30.0F) && (pitch < 30.0F))
{
rLED = LED_OFF;
gLED = LED_ON;
+
if(drdy)
{
//Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus
@@ -335,6 +410,14 @@
//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++;
+ }
+
//PID Block
//Error signal
currentError = (setPoint - pitch);
@@ -358,10 +441,22 @@
//Derivative term, dt is included in KD
derivative = (currentError - previousError);
+
//Set right/left pulsewidth
//Just balance for now, so both left/right are the same
pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative));
+
+ if(pingReady)
+ {
+ //Get error signal
+ pingReady = false;
+ pingCurrentError = PING_SP - cm;
+ }
+
+ //pulseWidth += pingCurrentError * -1.0F;
+
+
//Clamp to maximum duty cycle and check for forward/reverse
//based on sign of error signal (negation of pitch since setPoint = 0)
if(pulseWidth > MAX_PULSEWIDTH_US)
@@ -404,6 +499,49 @@
loopPulse = !loopPulse;
}
}
+
+ rightPwm.pulsewidth_us(0);
+ leftPwm.pulsewidth_us(0);
+
+ rLED = LED_ON;
+ gLED = LED_ON;
+ wait(0.25);
+ rLED = LED_OFF;
+ gLED = LED_OFF;
+ wait(0.25);
+
+ if(!uSDdetect && (samples > 0))
+ {
+ 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");
+ }
+ }
}
}
else