Carlos Orendain / Mbed 2 deprecated SELF_BALANCING_BOT

Self-Balancing bot for MAX32630 and two wheel car.

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

main.cpp@10:71c31197d55a, 2017-01-25 (annotated)

Committer:
j3
Date:
Wed Jan 25 19:44:43 2017 +0000
Revision:
10:71c31197d55a
Parent:
9:59add4a4372c
Child:
11:371026db1dcd
divide by count for averaging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j3 0:f7e385400dec 1 /**********************************************************************
j3 0:f7e385400dec 2 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
j3 0:f7e385400dec 3 *
j3 0:f7e385400dec 4 * Permission is hereby granted, free of charge, to any person obtaining a
j3 0:f7e385400dec 5 * copy of this software and associated documentation files (the "Software"),
j3 0:f7e385400dec 6 * to deal in the Software without restriction, including without limitation
j3 0:f7e385400dec 7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
j3 0:f7e385400dec 8 * and/or sell copies of the Software, and to permit persons to whom the
j3 0:f7e385400dec 9 * Software is furnished to do so, subject to the following conditions:
j3 0:f7e385400dec 10 *
j3 0:f7e385400dec 11 * The above copyright notice and this permission notice shall be included
j3 0:f7e385400dec 12 * in all copies or substantial portions of the Software.
j3 0:f7e385400dec 13 *
j3 0:f7e385400dec 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
j3 0:f7e385400dec 15 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
j3 0:f7e385400dec 16 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
j3 0:f7e385400dec 17 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
j3 0:f7e385400dec 18 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
j3 0:f7e385400dec 19 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
j3 0:f7e385400dec 20 * OTHER DEALINGS IN THE SOFTWARE.
j3 0:f7e385400dec 21 *
j3 0:f7e385400dec 22 * Except as contained in this notice, the name of Maxim Integrated
j3 0:f7e385400dec 23 * Products, Inc. shall not be used except as stated in the Maxim Integrated
j3 0:f7e385400dec 24 * Products, Inc. Branding Policy.
j3 0:f7e385400dec 25 *
j3 0:f7e385400dec 26 * The mere transfer of this software does not imply any licenses
j3 0:f7e385400dec 27 * of trade secrets, proprietary technology, copyrights, patents,
j3 0:f7e385400dec 28 * trademarks, maskwork rights, or any other form of intellectual
j3 0:f7e385400dec 29 * property whatsoever. Maxim Integrated Products, Inc. retains all
j3 0:f7e385400dec 30 * ownership rights.
j3 0:f7e385400dec 31 **********************************************************************/
j3 0:f7e385400dec 32
j3 0:f7e385400dec 33
j3 0:f7e385400dec 34 /*References
j3 0:f7e385400dec 35 *
j3 0:f7e385400dec 36 * Circuit Cellar Issue 316 November 2016
j3 0:f7e385400dec 37 * BalanceBot: A Self-Balancing, Two-Wheeled Robot
j3 0:f7e385400dec 38 *
j3 0:f7e385400dec 39 * Reading a IMU Without Kalman: The Complementary Filter
j3 0:f7e385400dec 40 * http://www.pieter-jan.com/node/11
j3 0:f7e385400dec 41 *
j3 6:9712c04e13bf 42 * The Balance Filter
j3 6:9712c04e13bf 43 * http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf
j3 6:9712c04e13bf 44 *
j3 0:f7e385400dec 45 */
j3 0:f7e385400dec 46
j3 0:f7e385400dec 47
j3 0:f7e385400dec 48 #include "mbed.h"
j3 0:f7e385400dec 49 #include "max32630fthr.h"
j3 0:f7e385400dec 50 #include "bmi160.h"
j3 3:7807a40b2459 51 #include "SDFileSystem.h"
j3 0:f7e385400dec 52
j3 6:9712c04e13bf 53
j3 6:9712c04e13bf 54 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT);
j3 4:4144ef007743 55
j3 4:4144ef007743 56 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX,
j3 6:9712c04e13bf 57 float * accY, float * accZ, int32_t * pw);
j3 4:4144ef007743 58
j3 0:f7e385400dec 59
j3 0:f7e385400dec 60 //Setup interrupt in from imu
j3 0:f7e385400dec 61 InterruptIn imuIntIn(P3_6);
j3 0:f7e385400dec 62 bool drdy = false;
j3 0:f7e385400dec 63 void imuISR()
j3 0:f7e385400dec 64 {
j3 0:f7e385400dec 65 drdy = true;
j3 0:f7e385400dec 66 }
j3 0:f7e385400dec 67
j3 0:f7e385400dec 68
j3 0:f7e385400dec 69 //Setup start/stop button
j3 0:f7e385400dec 70 DigitalIn startStopBtn(P2_3, PullUp);
j3 0:f7e385400dec 71 InterruptIn startStop(P2_3);
j3 0:f7e385400dec 72 bool start = false;
j3 0:f7e385400dec 73 void startStopISR()
j3 0:f7e385400dec 74 {
j3 3:7807a40b2459 75 start = !start;
j3 3:7807a40b2459 76 }
j3 3:7807a40b2459 77
j3 3:7807a40b2459 78
j3 3:7807a40b2459 79 //Setup callibrate button
j3 3:7807a40b2459 80 DigitalIn callibrateBtn(P5_2, PullUp);
j3 3:7807a40b2459 81 InterruptIn callibrate(P5_2);
j3 3:7807a40b2459 82 bool cal = false;
j3 3:7807a40b2459 83 void callibrateISR()
j3 3:7807a40b2459 84 {
j3 3:7807a40b2459 85 cal = !cal;
j3 3:7807a40b2459 86 }
j3 3:7807a40b2459 87
j3 3:7807a40b2459 88
j3 3:7807a40b2459 89 //Ping sensor trigger
j3 3:7807a40b2459 90 DigitalOut pingTrigger(P3_2, 0);
j3 3:7807a40b2459 91 Ticker pingTriggerTimer;
j3 3:7807a40b2459 92
j3 3:7807a40b2459 93 void triggerPing()
j3 3:7807a40b2459 94 {
j3 3:7807a40b2459 95 pingTrigger = !pingTrigger;
j3 3:7807a40b2459 96 wait_us(1);
j3 3:7807a40b2459 97 pingTrigger = !pingTrigger;
j3 3:7807a40b2459 98 }
j3 3:7807a40b2459 99
j3 3:7807a40b2459 100 DigitalIn p51(P5_1, PullNone);
j3 3:7807a40b2459 101 InterruptIn pingEchoRiseInt(P5_1);
j3 3:7807a40b2459 102 DigitalIn p45(P4_5, PullNone);
j3 3:7807a40b2459 103 InterruptIn pingEchoFallInt(P4_5);
j3 3:7807a40b2459 104 Timer pingTimer;
j3 3:7807a40b2459 105
j3 3:7807a40b2459 106 float cm = 0;
j3 3:7807a40b2459 107 bool pingReady = false;
j3 3:7807a40b2459 108 const float US_PER_CM = 29.387;
j3 3:7807a40b2459 109
j3 3:7807a40b2459 110 void echoStartISR()
j3 3:7807a40b2459 111 {
j3 3:7807a40b2459 112 pingTimer.reset();
j3 3:7807a40b2459 113 }
j3 3:7807a40b2459 114
j3 3:7807a40b2459 115 void echoStopISR()
j3 3:7807a40b2459 116 {
j3 3:7807a40b2459 117 uint32_t us = pingTimer.read_us()/2;
j3 3:7807a40b2459 118
j3 3:7807a40b2459 119 cm = (us/ US_PER_CM);
j3 3:7807a40b2459 120 pingReady = true;
j3 0:f7e385400dec 121 }
j3 0:f7e385400dec 122
j3 0:f7e385400dec 123
j3 0:f7e385400dec 124 int main()
j3 0:f7e385400dec 125 {
j3 0:f7e385400dec 126 MAX32630FTHR pegasus;
j3 0:f7e385400dec 127 pegasus.init(MAX32630FTHR::VIO_3V3);
j3 0:f7e385400dec 128
j3 0:f7e385400dec 129 static const float MAX_PULSEWIDTH_US = 40.0F;
j3 0:f7e385400dec 130 static const float MIN_PULSEWIDTH_US = -40.0F;
j3 0:f7e385400dec 131
j3 0:f7e385400dec 132 static const bool FORWARD = 0;
j3 0:f7e385400dec 133 static const bool REVERSE = 1;
j3 0:f7e385400dec 134
j3 0:f7e385400dec 135 //Setup left motor(from driver seat)
j3 0:f7e385400dec 136 DigitalOut leftDir(P5_6, FORWARD);
j3 0:f7e385400dec 137 PwmOut leftPwm(P4_0);
j3 0:f7e385400dec 138 leftPwm.period_us(40);
j3 0:f7e385400dec 139 leftPwm.pulsewidth_us(0);
j3 0:f7e385400dec 140
j3 0:f7e385400dec 141 //Make sure P4_2 and P4_3 are Hi-Z
j3 0:f7e385400dec 142 DigitalIn p42_hiZ(P4_2, PullNone);
j3 0:f7e385400dec 143 DigitalIn p43_hiZ(P4_3, PullNone);
j3 0:f7e385400dec 144
j3 0:f7e385400dec 145 //Setup right motor(from driver seat)
j3 0:f7e385400dec 146 DigitalOut rightDir(P5_4, FORWARD);
j3 0:f7e385400dec 147 PwmOut rightPwm(P5_5);
j3 0:f7e385400dec 148 rightPwm.period_us(40);
j3 0:f7e385400dec 149 rightPwm.pulsewidth_us(0);
j3 0:f7e385400dec 150
j3 0:f7e385400dec 151 //Turn off RGB LEDs
j3 0:f7e385400dec 152 DigitalOut rLED(LED1, LED_OFF);
j3 0:f7e385400dec 153 DigitalOut gLED(LED2, LED_OFF);
j3 0:f7e385400dec 154 DigitalOut bLED(LED3, LED_OFF);
j3 0:f7e385400dec 155
j3 3:7807a40b2459 156 uint32_t failures = 0;
j3 3:7807a40b2459 157
j3 3:7807a40b2459 158 DigitalIn uSDdetect(P2_2, PullUp);
j3 5:f66e4666de56 159 static const uint32_t N = 14400;
j3 3:7807a40b2459 160 uint32_t samples = 0;
j3 4:4144ef007743 161 float accYaxisBuff[N];
j3 4:4144ef007743 162 float accZaxisBuff[N];
j3 4:4144ef007743 163 float gyroXaxisBuff[N];
j3 6:9712c04e13bf 164 int32_t pulseWidthBuff[N];
j3 3:7807a40b2459 165
j3 0:f7e385400dec 166 //Setup I2C bus for IMU
j3 0:f7e385400dec 167 I2C i2cBus(P5_7, P6_0);
j3 0:f7e385400dec 168 i2cBus.frequency(400000);
j3 0:f7e385400dec 169
j3 0:f7e385400dec 170 //Get IMU instance and configure it
j3 0:f7e385400dec 171 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
j3 0:f7e385400dec 172
j3 0:f7e385400dec 173 BMI160::AccConfig accConfig;
j3 0:f7e385400dec 174 BMI160::AccConfig accConfigRead;
j3 6:9712c04e13bf 175 accConfig.range = BMI160::SENS_4G;
j3 0:f7e385400dec 176 accConfig.us = BMI160::ACC_US_OFF;
j3 0:f7e385400dec 177 accConfig.bwp = BMI160::ACC_BWP_2;
j3 6:9712c04e13bf 178 accConfig.odr = BMI160::ACC_ODR_11;
j3 0:f7e385400dec 179 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 180 {
j3 0:f7e385400dec 181 if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 182 {
j3 0:f7e385400dec 183 if((accConfig.range != accConfigRead.range) ||
j3 0:f7e385400dec 184 (accConfig.us != accConfigRead.us) ||
j3 0:f7e385400dec 185 (accConfig.bwp != accConfigRead.bwp) ||
j3 0:f7e385400dec 186 (accConfig.odr != accConfigRead.odr))
j3 0:f7e385400dec 187 {
j3 0:f7e385400dec 188 printf("ACC read data desn't equal set data\n\n");
j3 0:f7e385400dec 189 printf("ACC Set Range = %d\n", accConfig.range);
j3 0:f7e385400dec 190 printf("ACC Set UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 191 printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 192 printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 193 printf("ACC Read Range = %d\n", accConfigRead.range);
j3 0:f7e385400dec 194 printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
j3 0:f7e385400dec 195 printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
j3 0:f7e385400dec 196 printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
j3 0:f7e385400dec 197 failures++;
j3 0:f7e385400dec 198 }
j3 0:f7e385400dec 199
j3 0:f7e385400dec 200 }
j3 0:f7e385400dec 201 else
j3 0:f7e385400dec 202 {
j3 0:f7e385400dec 203 printf("Failed to read back accelerometer configuration\n");
j3 0:f7e385400dec 204 failures++;
j3 0:f7e385400dec 205 }
j3 0:f7e385400dec 206 }
j3 0:f7e385400dec 207 else
j3 0:f7e385400dec 208 {
j3 0:f7e385400dec 209 printf("Failed to set accelerometer configuration\n");
j3 0:f7e385400dec 210 failures++;
j3 0:f7e385400dec 211 }
j3 0:f7e385400dec 212
j3 0:f7e385400dec 213 BMI160::GyroConfig gyroConfig;
j3 0:f7e385400dec 214 BMI160::GyroConfig gyroConfigRead;
j3 0:f7e385400dec 215 gyroConfig.range = BMI160::DPS_2000;
j3 0:f7e385400dec 216 gyroConfig.bwp = BMI160::GYRO_BWP_2;
j3 6:9712c04e13bf 217 gyroConfig.odr = BMI160::GYRO_ODR_11;
j3 0:f7e385400dec 218 if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 219 {
j3 0:f7e385400dec 220 if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 221 {
j3 0:f7e385400dec 222 if((gyroConfig.range != gyroConfigRead.range) ||
j3 0:f7e385400dec 223 (gyroConfig.bwp != gyroConfigRead.bwp) ||
j3 0:f7e385400dec 224 (gyroConfig.odr != gyroConfigRead.odr))
j3 0:f7e385400dec 225 {
j3 0:f7e385400dec 226 printf("GYRO read data desn't equal set data\n\n");
j3 0:f7e385400dec 227 printf("GYRO Set Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 228 printf("GYRO Set BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 229 printf("GYRO Set OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 230 printf("GYRO Read Range = %d\n", gyroConfigRead.range);
j3 0:f7e385400dec 231 printf("GYRO Read BandWidthParam = %d\n", gyroConfigRead.bwp);
j3 0:f7e385400dec 232 printf("GYRO Read OutputDataRate = %d\n\n", gyroConfigRead.odr);
j3 0:f7e385400dec 233 failures++;
j3 0:f7e385400dec 234 }
j3 0:f7e385400dec 235
j3 0:f7e385400dec 236 }
j3 0:f7e385400dec 237 else
j3 0:f7e385400dec 238 {
j3 0:f7e385400dec 239 printf("Failed to read back gyroscope configuration\n");
j3 0:f7e385400dec 240 failures++;
j3 0:f7e385400dec 241 }
j3 0:f7e385400dec 242 }
j3 0:f7e385400dec 243 else
j3 0:f7e385400dec 244 {
j3 0:f7e385400dec 245 printf("Failed to set gyroscope configuration\n");
j3 0:f7e385400dec 246 failures++;
j3 0:f7e385400dec 247 }
j3 0:f7e385400dec 248
j3 0:f7e385400dec 249 //Power up sensors in normal mode
j3 0:f7e385400dec 250 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 251 {
j3 0:f7e385400dec 252 printf("Failed to set gyroscope power mode\n");
j3 0:f7e385400dec 253 failures++;
j3 0:f7e385400dec 254 }
j3 0:f7e385400dec 255
j3 0:f7e385400dec 256 wait(0.1);
j3 0:f7e385400dec 257
j3 0:f7e385400dec 258 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 259 {
j3 0:f7e385400dec 260 printf("Failed to set accelerometer power mode\n");
j3 0:f7e385400dec 261 failures++;
j3 0:f7e385400dec 262 }
j3 0:f7e385400dec 263
j3 0:f7e385400dec 264 if(failures == 0)
j3 0:f7e385400dec 265 {
j3 0:f7e385400dec 266 printf("ACC Range = %d\n", accConfig.range);
j3 0:f7e385400dec 267 printf("ACC UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 268 printf("ACC BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 269 printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 270 printf("GYRO Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 271 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 272 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 273 wait(1.0);
j3 0:f7e385400dec 274
j3 0:f7e385400dec 275 //Sensor data vars
j3 0:f7e385400dec 276 BMI160::SensorData accData;
j3 0:f7e385400dec 277 BMI160::SensorData gyroData;
j3 0:f7e385400dec 278 BMI160::SensorTime sensorTime;
j3 0:f7e385400dec 279
j3 0:f7e385400dec 280 //Complementary Filter vars, filter weight K
j3 8:810396d34da8 281 static const float K = 0.9965F;
j3 0:f7e385400dec 282 float pitch = 0.0F;
j3 0:f7e385400dec 283
j3 0:f7e385400dec 284 //PID coefficients
j3 6:9712c04e13bf 285
j3 6:9712c04e13bf 286 static const float DT = 0.00125F;
j3 8:810396d34da8 287 static const float KP = 1.85F;
j3 8:810396d34da8 288 static const float KI = (20.0F*DT);
j3 8:810396d34da8 289 static const float KD = (0.050F/DT);
j3 1:e9dd53326ba1 290 float setPoint = 0.0F;
j3 4:4144ef007743 291 float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT};
j3 0:f7e385400dec 292
j3 0:f7e385400dec 293 //Control loop vars
j3 0:f7e385400dec 294 float currentError, previousError;
j3 0:f7e385400dec 295 float integral = 0.0F;
j3 0:f7e385400dec 296 float derivative = 0.0F;
j3 0:f7e385400dec 297 float pulseWidth;
j3 0:f7e385400dec 298
j3 0:f7e385400dec 299 //Enable data ready interrupt from imu for constant loop timming
j3 0:f7e385400dec 300 imu.writeRegister(BMI160::INT_EN_1, 0x10);
j3 0:f7e385400dec 301 //Active High PushPull output on INT1
j3 0:f7e385400dec 302 imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A);
j3 0:f7e385400dec 303 //Map data ready interrupt to INT1
j3 0:f7e385400dec 304 imu.writeRegister(BMI160::INT_MAP_1, 0x80);
j3 3:7807a40b2459 305
j3 1:e9dd53326ba1 306 //Tie INT1 to callback fx
j3 0:f7e385400dec 307 imuIntIn.rise(&imuISR);
j3 0:f7e385400dec 308
j3 1:e9dd53326ba1 309 //Tie SW2 to callback fx
j3 0:f7e385400dec 310 startStop.fall(&startStopISR);
j3 0:f7e385400dec 311
j3 3:7807a40b2459 312 //Tie P5_2 to callback fx
j3 3:7807a40b2459 313 callibrate.fall(&callibrateISR);
j3 3:7807a40b2459 314 bool firstCal = true;
j3 3:7807a40b2459 315
j3 3:7807a40b2459 316 //Callbacks for echo measurement
j3 3:7807a40b2459 317 pingEchoRiseInt.fall(&echoStartISR);
j3 3:7807a40b2459 318 pingEchoFallInt.rise(&echoStopISR);
j3 3:7807a40b2459 319
j3 3:7807a40b2459 320 //Timer for echo measurements
j3 3:7807a40b2459 321 pingTimer.start();
j3 3:7807a40b2459 322
j3 3:7807a40b2459 323 //Timer for trigger
j3 3:7807a40b2459 324 pingTriggerTimer.attach(&triggerPing, 0.05);
j3 3:7807a40b2459 325
j3 3:7807a40b2459 326 //Position control vars/constants
j3 3:7807a40b2459 327 static const float PING_SP = 20.0F;
j3 4:4144ef007743 328 static const float PING_KP = 0.0F;
j3 3:7807a40b2459 329 float pingCurrentError = 0.0F;
j3 3:7807a40b2459 330
j3 0:f7e385400dec 331 //control loop timming indicator
j3 0:f7e385400dec 332 DigitalOut loopPulse(P5_3, 0);
j3 0:f7e385400dec 333
j3 1:e9dd53326ba1 334 //Count for averaging setPoint offset, 2 seconds of data
j3 1:e9dd53326ba1 335 uint32_t offsetCount = 0;
j3 1:e9dd53326ba1 336
j3 0:f7e385400dec 337 while(1)
j3 0:f7e385400dec 338 {
j3 0:f7e385400dec 339
j3 3:7807a40b2459 340 if(cal || (firstCal == true))
j3 1:e9dd53326ba1 341 {
j3 3:7807a40b2459 342 cal = false;
j3 3:7807a40b2459 343 firstCal = false;
j3 3:7807a40b2459 344 pitch = 0.0F;
j3 3:7807a40b2459 345 setPoint = 0.0F;
j3 3:7807a40b2459 346
j3 3:7807a40b2459 347 rLED = LED_ON;
j3 3:7807a40b2459 348 gLED = LED_ON;
j3 3:7807a40b2459 349
j3 6:9712c04e13bf 350 while(offsetCount < 1600)
j3 1:e9dd53326ba1 351 {
j3 3:7807a40b2459 352 if(drdy)
j3 3:7807a40b2459 353 {
j3 3:7807a40b2459 354 //Clear data ready flag
j3 3:7807a40b2459 355 drdy = false;
j3 3:7807a40b2459 356
j3 3:7807a40b2459 357 //Read feedback sensors
j3 3:7807a40b2459 358 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
j3 3:7807a40b2459 359
j3 6:9712c04e13bf 360 //Feedback Block, pitch estimate in degrees
j3 6:9712c04e13bf 361 pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
j3 3:7807a40b2459 362
j3 3:7807a40b2459 363 //Accumalate pitch measurements
j3 3:7807a40b2459 364 setPoint += pitch;
j3 3:7807a40b2459 365 offsetCount++;
j3 3:7807a40b2459 366 }
j3 1:e9dd53326ba1 367 }
j3 3:7807a40b2459 368
j3 3:7807a40b2459 369 //Average measurements
j3 10:71c31197d55a 370 setPoint = setPoint/offsetCount;
j3 3:7807a40b2459 371 printf("setPoint = %5.2f\n\n", setPoint);
j3 3:7807a40b2459 372 //Clear count for next time
j3 3:7807a40b2459 373 offsetCount = 0;
j3 1:e9dd53326ba1 374 }
j3 1:e9dd53326ba1 375
j3 1:e9dd53326ba1 376
j3 8:810396d34da8 377 while(start && (pitch > -20.0F) && (pitch < 20.0F))
j3 0:f7e385400dec 378 {
j3 0:f7e385400dec 379 rLED = LED_OFF;
j3 0:f7e385400dec 380 gLED = LED_ON;
j3 3:7807a40b2459 381
j3 0:f7e385400dec 382 if(drdy)
j3 0:f7e385400dec 383 {
j3 0:f7e385400dec 384 //Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus
j3 0:f7e385400dec 385 //At 1600Hz ODR, ~73% of loop time is active doing something
j3 0:f7e385400dec 386 loopPulse = !loopPulse;
j3 0:f7e385400dec 387
j3 0:f7e385400dec 388 //Clear data ready flag
j3 0:f7e385400dec 389 drdy = false;
j3 0:f7e385400dec 390
j3 0:f7e385400dec 391 //Read feedback sensors
j3 0:f7e385400dec 392 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
j3 0:f7e385400dec 393
j3 6:9712c04e13bf 394 //Feedback Block, pitch estimate in degrees
j3 6:9712c04e13bf 395 pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
j3 6:9712c04e13bf 396
j3 0:f7e385400dec 397 //PID Block
j3 0:f7e385400dec 398 //Error signal
j3 1:e9dd53326ba1 399 currentError = (setPoint - pitch);
j3 0:f7e385400dec 400
j3 0:f7e385400dec 401 //Integral term, dt is included in KI
j3 0:f7e385400dec 402 //If statement addresses integral windup
j3 0:f7e385400dec 403 if(currentError == 0.0F)
j3 0:f7e385400dec 404 {
j3 0:f7e385400dec 405 integral = 0.0F;
j3 0:f7e385400dec 406 }
j3 0:f7e385400dec 407 else if(((currentError < 0.0F) && (previousError > 0.0F)) ||
j3 0:f7e385400dec 408 ((currentError > 0.0F) && (previousError < 0.0F)))
j3 0:f7e385400dec 409 {
j3 0:f7e385400dec 410 integral = 0.0F;
j3 0:f7e385400dec 411 }
j3 0:f7e385400dec 412 else
j3 0:f7e385400dec 413 {
j3 0:f7e385400dec 414 integral = (integral + currentError);
j3 0:f7e385400dec 415 }
j3 0:f7e385400dec 416
j3 0:f7e385400dec 417 //Derivative term, dt is included in KD
j3 0:f7e385400dec 418 derivative = (currentError - previousError);
j3 0:f7e385400dec 419
j3 3:7807a40b2459 420
j3 0:f7e385400dec 421 //Set right/left pulsewidth
j3 0:f7e385400dec 422 //Just balance for now, so both left/right are the same
j3 0:f7e385400dec 423 pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative));
j3 0:f7e385400dec 424
j3 3:7807a40b2459 425
j3 3:7807a40b2459 426 if(pingReady)
j3 3:7807a40b2459 427 {
j3 3:7807a40b2459 428 //Get error signal
j3 3:7807a40b2459 429 pingReady = false;
j3 3:7807a40b2459 430 pingCurrentError = PING_SP - cm;
j3 3:7807a40b2459 431 }
j3 3:7807a40b2459 432
j3 4:4144ef007743 433 pulseWidth += (pingCurrentError * PING_KP);
j3 3:7807a40b2459 434
j3 3:7807a40b2459 435
j3 0:f7e385400dec 436 //Clamp to maximum duty cycle and check for forward/reverse
j3 1:e9dd53326ba1 437 //based on sign of error signal (negation of pitch since setPoint = 0)
j3 0:f7e385400dec 438 if(pulseWidth > MAX_PULSEWIDTH_US)
j3 0:f7e385400dec 439 {
j3 0:f7e385400dec 440 rightDir = FORWARD;
j3 0:f7e385400dec 441 leftDir = FORWARD;
j3 4:4144ef007743 442 pulseWidth = 40.0F;
j3 0:f7e385400dec 443 rightPwm.pulsewidth_us(40);
j3 0:f7e385400dec 444 leftPwm.pulsewidth_us(40);
j3 0:f7e385400dec 445 }
j3 0:f7e385400dec 446 else if(pulseWidth < MIN_PULSEWIDTH_US)
j3 0:f7e385400dec 447 {
j3 0:f7e385400dec 448 rightDir = REVERSE;
j3 0:f7e385400dec 449 leftDir = REVERSE;
j3 4:4144ef007743 450 pulseWidth = -40.0F;
j3 0:f7e385400dec 451 rightPwm.pulsewidth_us(40);
j3 0:f7e385400dec 452 leftPwm.pulsewidth_us(40);
j3 0:f7e385400dec 453 }
j3 0:f7e385400dec 454 else
j3 0:f7e385400dec 455 {
j3 0:f7e385400dec 456 if(pulseWidth < 0.0F)
j3 0:f7e385400dec 457 {
j3 0:f7e385400dec 458 rightDir = REVERSE;
j3 0:f7e385400dec 459 leftDir = REVERSE;
j3 4:4144ef007743 460 rightPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
j3 4:4144ef007743 461 leftPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
j3 0:f7e385400dec 462 }
j3 0:f7e385400dec 463 else
j3 0:f7e385400dec 464 {
j3 0:f7e385400dec 465 rightDir = FORWARD;
j3 0:f7e385400dec 466 leftDir = FORWARD;
j3 0:f7e385400dec 467 rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 468 leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 469 }
j3 0:f7e385400dec 470 }
j3 0:f7e385400dec 471
j3 4:4144ef007743 472 if(samples < N)
j3 4:4144ef007743 473 {
j3 4:4144ef007743 474 accYaxisBuff[samples] = accData.yAxis.scaled;
j3 4:4144ef007743 475 accZaxisBuff[samples] = accData.zAxis.scaled;
j3 4:4144ef007743 476 gyroXaxisBuff[samples] = gyroData.xAxis.scaled;
j3 6:9712c04e13bf 477 pulseWidthBuff[samples] = static_cast<int32_t>(pulseWidth);
j3 4:4144ef007743 478 samples++;
j3 4:4144ef007743 479 }
j3 4:4144ef007743 480
j3 0:f7e385400dec 481 //save current error for next loop
j3 0:f7e385400dec 482 previousError = currentError;
j3 4:4144ef007743 483
j3 0:f7e385400dec 484 //Stop
j3 0:f7e385400dec 485 loopPulse = !loopPulse;
j3 0:f7e385400dec 486 }
j3 0:f7e385400dec 487 }
j3 3:7807a40b2459 488
j3 8:810396d34da8 489 if((pitch > 20.0F) || (pitch < -20.0F))
j3 4:4144ef007743 490 {
j3 4:4144ef007743 491 start = false;
j3 4:4144ef007743 492 }
j3 8:810396d34da8 493
j3 4:4144ef007743 494 pitch = 0.0F;
j3 4:4144ef007743 495 integral = 0.0F;
j3 4:4144ef007743 496 previousError = 0.0F;
j3 3:7807a40b2459 497 rightPwm.pulsewidth_us(0);
j3 3:7807a40b2459 498 leftPwm.pulsewidth_us(0);
j3 3:7807a40b2459 499
j3 3:7807a40b2459 500 rLED = LED_ON;
j3 3:7807a40b2459 501 gLED = LED_ON;
j3 3:7807a40b2459 502 wait(0.25);
j3 3:7807a40b2459 503 rLED = LED_OFF;
j3 3:7807a40b2459 504 gLED = LED_OFF;
j3 3:7807a40b2459 505 wait(0.25);
j3 3:7807a40b2459 506
j3 3:7807a40b2459 507 if(!uSDdetect && (samples > 0))
j3 4:4144ef007743 508 {
j3 4:4144ef007743 509 loopCoeffs[0] = setPoint;
j3 4:4144ef007743 510 bLED = LED_ON;
j3 4:4144ef007743 511 saveData(samples, loopCoeffs, gyroXaxisBuff, accYaxisBuff, accZaxisBuff, pulseWidthBuff);
j3 3:7807a40b2459 512 samples = 0;
j3 4:4144ef007743 513 bLED = LED_OFF;
j3 3:7807a40b2459 514 }
j3 0:f7e385400dec 515 }
j3 0:f7e385400dec 516 }
j3 0:f7e385400dec 517 else
j3 0:f7e385400dec 518 {
j3 0:f7e385400dec 519 while(1)
j3 0:f7e385400dec 520 {
j3 0:f7e385400dec 521 rLED = !rLED;
j3 0:f7e385400dec 522 wait(0.25);
j3 0:f7e385400dec 523 }
j3 0:f7e385400dec 524 }
j3 0:f7e385400dec 525 }
j3 4:4144ef007743 526
j3 4:4144ef007743 527
j3 4:4144ef007743 528 //*****************************************************************************
j3 4:4144ef007743 529 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX,
j3 6:9712c04e13bf 530 float * accY, float * accZ, int32_t * pw)
j3 4:4144ef007743 531 {
j3 4:4144ef007743 532 SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs
j3 4:4144ef007743 533 FILE *fp;
j3 4:4144ef007743 534
j3 4:4144ef007743 535 fp = fopen("/sd/balBot.txt", "a");
j3 4:4144ef007743 536 if(fp != NULL)
j3 4:4144ef007743 537 {
j3 8:810396d34da8 538 fprintf(fp, "Samples,%d,,,,,\n", numSamples);
j3 4:4144ef007743 539 fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]);
j3 4:4144ef007743 540 fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]);
j3 4:4144ef007743 541 fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]);
j3 4:4144ef007743 542 fprintf(fp, "KI, %f,,,,,\n", loopCoeffs[3]);
j3 4:4144ef007743 543 fprintf(fp, "KD, %f,,,,,\n", loopCoeffs[4]);
j3 4:4144ef007743 544 fprintf(fp, "DT, %f,,,,,\n", loopCoeffs[5]);
j3 4:4144ef007743 545 fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n");
j3 4:4144ef007743 546
j3 4:4144ef007743 547 float accPitch = 0.0F;
j3 5:f66e4666de56 548 float gyroDegrees = 0.0F;
j3 4:4144ef007743 549 float pitch = 0.0F;
j3 6:9712c04e13bf 550 float K = loopCoeffs[1];
j3 6:9712c04e13bf 551 float DT = loopCoeffs[5];
j3 4:4144ef007743 552
j3 4:4144ef007743 553 for(uint32_t idx = 0; idx < numSamples; idx++)
j3 4:4144ef007743 554 {
j3 6:9712c04e13bf 555 fprintf(fp, "%f,", idx*DT);
j3 4:4144ef007743 556 fprintf(fp, "%5.4f,", accY[idx]);
j3 4:4144ef007743 557 fprintf(fp, "%6.2f,", gyroX[idx]);
j3 6:9712c04e13bf 558 gyroDegrees += (gyroX[idx] * DT);
j3 5:f66e4666de56 559 fprintf(fp, "%5.2f,", gyroDegrees);
j3 4:4144ef007743 560 accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx]));
j3 4:4144ef007743 561 fprintf(fp, "%5.2f,", accPitch);
j3 6:9712c04e13bf 562 pitch = compFilter(K, pitch, gyroX[idx], accY[idx], accZ[idx], DT);
j3 5:f66e4666de56 563 fprintf(fp, "%5.2f,", pitch);
j3 6:9712c04e13bf 564 fprintf(fp, "%d", pw[idx]);
j3 4:4144ef007743 565 fprintf(fp, "\n");
j3 4:4144ef007743 566 }
j3 4:4144ef007743 567 fprintf(fp, "\n");
j3 4:4144ef007743 568 fclose(fp);
j3 4:4144ef007743 569 }
j3 4:4144ef007743 570 else
j3 4:4144ef007743 571 {
j3 4:4144ef007743 572 printf("Failed to open file\n");
j3 4:4144ef007743 573 }
j3 4:4144ef007743 574 }
j3 6:9712c04e13bf 575
j3 6:9712c04e13bf 576
j3 6:9712c04e13bf 577 //*****************************************************************************
j3 6:9712c04e13bf 578 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT)
j3 6:9712c04e13bf 579 {
j3 6:9712c04e13bf 580 return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
j3 6:9712c04e13bf 581 }