Carlos Orendain / Mbed 2 deprecated SELF_BALANCING_BOT

Self-Balancing bot for MAX32630 and two wheel car.

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

main.cpp@14:988b8d294259, 2020-08-23 (annotated)

Committer:
CharlieO21
Date:
Sun Aug 23 18:43:13 2020 +0000
Revision:
14:988b8d294259
Parent:
12:0fb3a03aeecb 
Change pin setting to add pwm capabilities to the 4 control pins.

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
CharlieO21 14:988b8d294259 129 static const float MAX_PULSEWIDTH_US = 50.0F;
CharlieO21 14:988b8d294259 130 static const float MIN_PULSEWIDTH_US = -50.0F;
CharlieO21 14:988b8d294259 131 static const int PERIOD_US = 100;
CharlieO21 14:988b8d294259 132 static const int MAX_PULSEWIDTH = 50;
j3 0:f7e385400dec 133
j3 0:f7e385400dec 134 static const bool FORWARD = 0;
j3 0:f7e385400dec 135 static const bool REVERSE = 1;
j3 0:f7e385400dec 136
j3 0:f7e385400dec 137 //Setup left motor(from driver seat)
CharlieO21 14:988b8d294259 138 //DigitalOut leftDir(P5_6, FORWARD);
CharlieO21 14:988b8d294259 139 PwmOut leftDir(P5_6);
CharlieO21 14:988b8d294259 140 leftDir.period_us(PERIOD_US);
CharlieO21 14:988b8d294259 141 leftDir.pulsewidth_us(0);
CharlieO21 14:988b8d294259 142
j3 0:f7e385400dec 143 PwmOut leftPwm(P4_0);
CharlieO21 14:988b8d294259 144 leftPwm.period_us(PERIOD_US);
j3 0:f7e385400dec 145 leftPwm.pulsewidth_us(0);
j3 0:f7e385400dec 146
j3 0:f7e385400dec 147 //Make sure P4_2 and P4_3 are Hi-Z
j3 0:f7e385400dec 148 DigitalIn p42_hiZ(P4_2, PullNone);
j3 0:f7e385400dec 149 DigitalIn p43_hiZ(P4_3, PullNone);
j3 0:f7e385400dec 150
j3 0:f7e385400dec 151 //Setup right motor(from driver seat)
CharlieO21 14:988b8d294259 152 //DigitalOut rightDir(P5_4, FORWARD);
CharlieO21 14:988b8d294259 153 PwmOut rightDir(P5_4);
CharlieO21 14:988b8d294259 154 rightDir.period_us(PERIOD_US);
CharlieO21 14:988b8d294259 155 rightDir.pulsewidth_us(0);
CharlieO21 14:988b8d294259 156
j3 0:f7e385400dec 157 PwmOut rightPwm(P5_5);
CharlieO21 14:988b8d294259 158 rightPwm.period_us(PERIOD_US);
j3 0:f7e385400dec 159 rightPwm.pulsewidth_us(0);
j3 0:f7e385400dec 160
j3 0:f7e385400dec 161 //Turn off RGB LEDs
j3 0:f7e385400dec 162 DigitalOut rLED(LED1, LED_OFF);
j3 0:f7e385400dec 163 DigitalOut gLED(LED2, LED_OFF);
j3 0:f7e385400dec 164 DigitalOut bLED(LED3, LED_OFF);
j3 0:f7e385400dec 165
j3 3:7807a40b2459 166 uint32_t failures = 0;
j3 3:7807a40b2459 167
j3 3:7807a40b2459 168 DigitalIn uSDdetect(P2_2, PullUp);
j3 5:f66e4666de56 169 static const uint32_t N = 14400;
j3 3:7807a40b2459 170 uint32_t samples = 0;
j3 4:4144ef007743 171 float accYaxisBuff[N];
j3 4:4144ef007743 172 float accZaxisBuff[N];
j3 4:4144ef007743 173 float gyroXaxisBuff[N];
j3 6:9712c04e13bf 174 int32_t pulseWidthBuff[N];
j3 3:7807a40b2459 175
j3 0:f7e385400dec 176 //Setup I2C bus for IMU
j3 0:f7e385400dec 177 I2C i2cBus(P5_7, P6_0);
j3 0:f7e385400dec 178 i2cBus.frequency(400000);
j3 0:f7e385400dec 179
j3 0:f7e385400dec 180 //Get IMU instance and configure it
j3 0:f7e385400dec 181 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
j3 0:f7e385400dec 182
j3 12:0fb3a03aeecb 183 //Power up sensors in normal mode
j3 12:0fb3a03aeecb 184 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 12:0fb3a03aeecb 185 {
j3 12:0fb3a03aeecb 186 printf("Failed to set gyroscope power mode\n");
j3 12:0fb3a03aeecb 187 failures++;
j3 12:0fb3a03aeecb 188 }
j3 12:0fb3a03aeecb 189
j3 12:0fb3a03aeecb 190 wait(0.1);
j3 12:0fb3a03aeecb 191
j3 12:0fb3a03aeecb 192 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 12:0fb3a03aeecb 193 {
j3 12:0fb3a03aeecb 194 printf("Failed to set accelerometer power mode\n");
j3 12:0fb3a03aeecb 195 failures++;
j3 12:0fb3a03aeecb 196 }
j3 12:0fb3a03aeecb 197 wait(0.1);
j3 12:0fb3a03aeecb 198
j3 0:f7e385400dec 199 BMI160::AccConfig accConfig;
j3 0:f7e385400dec 200 BMI160::AccConfig accConfigRead;
j3 6:9712c04e13bf 201 accConfig.range = BMI160::SENS_4G;
j3 0:f7e385400dec 202 accConfig.us = BMI160::ACC_US_OFF;
j3 0:f7e385400dec 203 accConfig.bwp = BMI160::ACC_BWP_2;
j3 6:9712c04e13bf 204 accConfig.odr = BMI160::ACC_ODR_11;
j3 0:f7e385400dec 205 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 206 {
j3 0:f7e385400dec 207 if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 208 {
j3 0:f7e385400dec 209 if((accConfig.range != accConfigRead.range) ||
j3 0:f7e385400dec 210 (accConfig.us != accConfigRead.us) ||
j3 0:f7e385400dec 211 (accConfig.bwp != accConfigRead.bwp) ||
j3 0:f7e385400dec 212 (accConfig.odr != accConfigRead.odr))
j3 0:f7e385400dec 213 {
j3 0:f7e385400dec 214 printf("ACC read data desn't equal set data\n\n");
j3 0:f7e385400dec 215 printf("ACC Set Range = %d\n", accConfig.range);
j3 0:f7e385400dec 216 printf("ACC Set UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 217 printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 218 printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 219 printf("ACC Read Range = %d\n", accConfigRead.range);
j3 0:f7e385400dec 220 printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
j3 0:f7e385400dec 221 printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
j3 0:f7e385400dec 222 printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
j3 0:f7e385400dec 223 failures++;
j3 0:f7e385400dec 224 }
j3 0:f7e385400dec 225
j3 0:f7e385400dec 226 }
j3 0:f7e385400dec 227 else
j3 0:f7e385400dec 228 {
j3 0:f7e385400dec 229 printf("Failed to read back accelerometer configuration\n");
j3 0:f7e385400dec 230 failures++;
j3 0:f7e385400dec 231 }
j3 0:f7e385400dec 232 }
j3 0:f7e385400dec 233 else
j3 0:f7e385400dec 234 {
j3 0:f7e385400dec 235 printf("Failed to set accelerometer configuration\n");
j3 0:f7e385400dec 236 failures++;
j3 0:f7e385400dec 237 }
j3 0:f7e385400dec 238
j3 0:f7e385400dec 239 BMI160::GyroConfig gyroConfig;
j3 0:f7e385400dec 240 BMI160::GyroConfig gyroConfigRead;
j3 0:f7e385400dec 241 gyroConfig.range = BMI160::DPS_2000;
j3 0:f7e385400dec 242 gyroConfig.bwp = BMI160::GYRO_BWP_2;
j3 6:9712c04e13bf 243 gyroConfig.odr = BMI160::GYRO_ODR_11;
j3 0:f7e385400dec 244 if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 245 {
j3 0:f7e385400dec 246 if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 247 {
j3 0:f7e385400dec 248 if((gyroConfig.range != gyroConfigRead.range) ||
j3 0:f7e385400dec 249 (gyroConfig.bwp != gyroConfigRead.bwp) ||
j3 0:f7e385400dec 250 (gyroConfig.odr != gyroConfigRead.odr))
j3 0:f7e385400dec 251 {
j3 0:f7e385400dec 252 printf("GYRO read data desn't equal set data\n\n");
j3 0:f7e385400dec 253 printf("GYRO Set Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 254 printf("GYRO Set BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 255 printf("GYRO Set OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 256 printf("GYRO Read Range = %d\n", gyroConfigRead.range);
j3 0:f7e385400dec 257 printf("GYRO Read BandWidthParam = %d\n", gyroConfigRead.bwp);
j3 0:f7e385400dec 258 printf("GYRO Read OutputDataRate = %d\n\n", gyroConfigRead.odr);
j3 0:f7e385400dec 259 failures++;
j3 0:f7e385400dec 260 }
j3 0:f7e385400dec 261
j3 0:f7e385400dec 262 }
j3 0:f7e385400dec 263 else
j3 0:f7e385400dec 264 {
j3 0:f7e385400dec 265 printf("Failed to read back gyroscope configuration\n");
j3 0:f7e385400dec 266 failures++;
j3 0:f7e385400dec 267 }
j3 0:f7e385400dec 268 }
j3 0:f7e385400dec 269 else
j3 0:f7e385400dec 270 {
j3 0:f7e385400dec 271 printf("Failed to set gyroscope configuration\n");
j3 0:f7e385400dec 272 failures++;
j3 0:f7e385400dec 273 }
j3 0:f7e385400dec 274
j3 0:f7e385400dec 275
j3 0:f7e385400dec 276 if(failures == 0)
j3 0:f7e385400dec 277 {
j3 0:f7e385400dec 278 printf("ACC Range = %d\n", accConfig.range);
j3 0:f7e385400dec 279 printf("ACC UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 280 printf("ACC BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 281 printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 282 printf("GYRO Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 283 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 284 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 285 wait(1.0);
j3 0:f7e385400dec 286
j3 0:f7e385400dec 287 //Sensor data vars
j3 0:f7e385400dec 288 BMI160::SensorData accData;
j3 0:f7e385400dec 289 BMI160::SensorData gyroData;
j3 0:f7e385400dec 290 BMI160::SensorTime sensorTime;
j3 0:f7e385400dec 291
j3 0:f7e385400dec 292 //Complementary Filter vars, filter weight K
j3 11:371026db1dcd 293 static const float K = 0.9978F;
j3 0:f7e385400dec 294 float pitch = 0.0F;
j3 0:f7e385400dec 295
j3 0:f7e385400dec 296 //PID coefficients
j3 11:371026db1dcd 297
j3 11:371026db1dcd 298 //tunning with Wilson, Kc = 4.2, pc = 0.166
j3 6:9712c04e13bf 299
j3 6:9712c04e13bf 300 static const float DT = 0.00125F;
j3 11:371026db1dcd 301 static const float KP = 2.5F;
j3 11:371026db1dcd 302 static const float KI = (30.0F*DT);
j3 11:371026db1dcd 303 static const float KD = (0.05F/DT);
j3 1:e9dd53326ba1 304 float setPoint = 0.0F;
j3 4:4144ef007743 305 float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT};
j3 0:f7e385400dec 306
j3 0:f7e385400dec 307 //Control loop vars
j3 0:f7e385400dec 308 float currentError, previousError;
j3 0:f7e385400dec 309 float integral = 0.0F;
j3 0:f7e385400dec 310 float derivative = 0.0F;
j3 0:f7e385400dec 311 float pulseWidth;
j3 0:f7e385400dec 312
j3 0:f7e385400dec 313 //Enable data ready interrupt from imu for constant loop timming
j3 0:f7e385400dec 314 imu.writeRegister(BMI160::INT_EN_1, 0x10);
j3 0:f7e385400dec 315 //Active High PushPull output on INT1
j3 0:f7e385400dec 316 imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A);
j3 0:f7e385400dec 317 //Map data ready interrupt to INT1
j3 0:f7e385400dec 318 imu.writeRegister(BMI160::INT_MAP_1, 0x80);
j3 3:7807a40b2459 319
j3 1:e9dd53326ba1 320 //Tie INT1 to callback fx
j3 0:f7e385400dec 321 imuIntIn.rise(&imuISR);
j3 0:f7e385400dec 322
j3 1:e9dd53326ba1 323 //Tie SW2 to callback fx
j3 0:f7e385400dec 324 startStop.fall(&startStopISR);
j3 0:f7e385400dec 325
j3 3:7807a40b2459 326 //Tie P5_2 to callback fx
j3 3:7807a40b2459 327 callibrate.fall(&callibrateISR);
j3 3:7807a40b2459 328 bool firstCal = true;
j3 3:7807a40b2459 329
j3 3:7807a40b2459 330 //Callbacks for echo measurement
CharlieO21 14:988b8d294259 331 //pingEchoRiseInt.fall(&echoStartISR);
CharlieO21 14:988b8d294259 332 //pingEchoFallInt.rise(&echoStopISR);
j3 3:7807a40b2459 333
j3 3:7807a40b2459 334 //Timer for echo measurements
CharlieO21 14:988b8d294259 335 //pingTimer.start();
j3 3:7807a40b2459 336
j3 3:7807a40b2459 337 //Timer for trigger
CharlieO21 14:988b8d294259 338 //pingTriggerTimer.attach(&triggerPing, 0.05);
j3 3:7807a40b2459 339
j3 3:7807a40b2459 340 //Position control vars/constants
j3 11:371026db1dcd 341 //static const float PING_SP = 20.0F;
j3 11:371026db1dcd 342 //static const float PING_KP = 0.0F;
j3 11:371026db1dcd 343 //float pingCurrentError = 0.0F;
j3 3:7807a40b2459 344
j3 0:f7e385400dec 345 //control loop timming indicator
j3 0:f7e385400dec 346 DigitalOut loopPulse(P5_3, 0);
j3 0:f7e385400dec 347
j3 1:e9dd53326ba1 348 //Count for averaging setPoint offset, 2 seconds of data
j3 1:e9dd53326ba1 349 uint32_t offsetCount = 0;
j3 1:e9dd53326ba1 350
j3 0:f7e385400dec 351 while(1)
j3 0:f7e385400dec 352 {
j3 0:f7e385400dec 353
j3 3:7807a40b2459 354 if(cal || (firstCal == true))
j3 1:e9dd53326ba1 355 {
j3 3:7807a40b2459 356 cal = false;
j3 3:7807a40b2459 357 firstCal = false;
j3 3:7807a40b2459 358 pitch = 0.0F;
j3 3:7807a40b2459 359 setPoint = 0.0F;
j3 3:7807a40b2459 360
j3 3:7807a40b2459 361 rLED = LED_ON;
j3 3:7807a40b2459 362 gLED = LED_ON;
j3 3:7807a40b2459 363
j3 6:9712c04e13bf 364 while(offsetCount < 1600)
j3 1:e9dd53326ba1 365 {
j3 3:7807a40b2459 366 if(drdy)
j3 3:7807a40b2459 367 {
j3 3:7807a40b2459 368 //Clear data ready flag
j3 3:7807a40b2459 369 drdy = false;
j3 3:7807a40b2459 370
j3 3:7807a40b2459 371 //Read feedback sensors
j3 3:7807a40b2459 372 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
j3 3:7807a40b2459 373
j3 6:9712c04e13bf 374 //Feedback Block, pitch estimate in degrees
j3 6:9712c04e13bf 375 pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
j3 3:7807a40b2459 376
j3 3:7807a40b2459 377 //Accumalate pitch measurements
j3 3:7807a40b2459 378 setPoint += pitch;
j3 3:7807a40b2459 379 offsetCount++;
j3 3:7807a40b2459 380 }
j3 1:e9dd53326ba1 381 }
j3 3:7807a40b2459 382
j3 3:7807a40b2459 383 //Average measurements
j3 10:71c31197d55a 384 setPoint = setPoint/offsetCount;
j3 3:7807a40b2459 385 printf("setPoint = %5.2f\n\n", setPoint);
j3 3:7807a40b2459 386 //Clear count for next time
j3 3:7807a40b2459 387 offsetCount = 0;
j3 1:e9dd53326ba1 388 }
j3 1:e9dd53326ba1 389
j3 1:e9dd53326ba1 390
j3 8:810396d34da8 391 while(start && (pitch > -20.0F) && (pitch < 20.0F))
CharlieO21 14:988b8d294259 392 {
j3 0:f7e385400dec 393 rLED = LED_OFF;
j3 0:f7e385400dec 394 gLED = LED_ON;
j3 3:7807a40b2459 395
j3 0:f7e385400dec 396 if(drdy)
j3 0:f7e385400dec 397 {
j3 0:f7e385400dec 398 //Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus
j3 0:f7e385400dec 399 //At 1600Hz ODR, ~73% of loop time is active doing something
j3 0:f7e385400dec 400 loopPulse = !loopPulse;
j3 0:f7e385400dec 401
j3 0:f7e385400dec 402 //Clear data ready flag
j3 0:f7e385400dec 403 drdy = false;
j3 0:f7e385400dec 404
j3 0:f7e385400dec 405 //Read feedback sensors
j3 0:f7e385400dec 406 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
j3 0:f7e385400dec 407
j3 6:9712c04e13bf 408 //Feedback Block, pitch estimate in degrees
j3 6:9712c04e13bf 409 pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
j3 6:9712c04e13bf 410
j3 0:f7e385400dec 411 //PID Block
j3 0:f7e385400dec 412 //Error signal
j3 1:e9dd53326ba1 413 currentError = (setPoint - pitch);
j3 0:f7e385400dec 414
j3 12:0fb3a03aeecb 415 //Integral term, dt is included in KI
j3 11:371026db1dcd 416 integral = (integral + currentError);
j3 0:f7e385400dec 417
j3 0:f7e385400dec 418 //Derivative term, dt is included in KD
j3 0:f7e385400dec 419 derivative = (currentError - previousError);
j3 0:f7e385400dec 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 11:371026db1dcd 426 /*
j3 3:7807a40b2459 427 if(pingReady)
j3 3:7807a40b2459 428 {
j3 3:7807a40b2459 429 //Get error signal
j3 3:7807a40b2459 430 pingReady = false;
j3 3:7807a40b2459 431 pingCurrentError = PING_SP - cm;
j3 3:7807a40b2459 432 }
j3 3:7807a40b2459 433
j3 4:4144ef007743 434 pulseWidth += (pingCurrentError * PING_KP);
j3 11:371026db1dcd 435 */
j3 3:7807a40b2459 436
j3 3:7807a40b2459 437
j3 0:f7e385400dec 438 //Clamp to maximum duty cycle and check for forward/reverse
j3 1:e9dd53326ba1 439 //based on sign of error signal (negation of pitch since setPoint = 0)
j3 0:f7e385400dec 440 if(pulseWidth > MAX_PULSEWIDTH_US)
j3 0:f7e385400dec 441 {
CharlieO21 14:988b8d294259 442 //rightDir = FORWARD;
CharlieO21 14:988b8d294259 443 //leftDir = FORWARD;
CharlieO21 14:988b8d294259 444 pulseWidth = MAX_PULSEWIDTH_US;
CharlieO21 14:988b8d294259 445 rightPwm.pulsewidth_us(MAX_PULSEWIDTH);
CharlieO21 14:988b8d294259 446 leftPwm.pulsewidth_us(MAX_PULSEWIDTH);
j3 0:f7e385400dec 447 }
j3 0:f7e385400dec 448 else if(pulseWidth < MIN_PULSEWIDTH_US)
j3 0:f7e385400dec 449 {
CharlieO21 14:988b8d294259 450 //rightDir = REVERSE;
CharlieO21 14:988b8d294259 451 //leftDir = REVERSE;
CharlieO21 14:988b8d294259 452 pulseWidth = MIN_PULSEWIDTH_US;
CharlieO21 14:988b8d294259 453 rightDir.pulsewidth_us(MAX_PULSEWIDTH);
CharlieO21 14:988b8d294259 454 leftDir.pulsewidth_us(MAX_PULSEWIDTH);
j3 0:f7e385400dec 455 }
j3 0:f7e385400dec 456 else
j3 0:f7e385400dec 457 {
j3 0:f7e385400dec 458 if(pulseWidth < 0.0F)
j3 0:f7e385400dec 459 {
CharlieO21 14:988b8d294259 460 //rightDir = REVERSE;
CharlieO21 14:988b8d294259 461 //leftDir = REVERSE;
CharlieO21 14:988b8d294259 462 rightDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
CharlieO21 14:988b8d294259 463 leftDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
j3 0:f7e385400dec 464 }
j3 0:f7e385400dec 465 else
j3 0:f7e385400dec 466 {
CharlieO21 14:988b8d294259 467 //rightDir = FORWARD;
CharlieO21 14:988b8d294259 468 //leftDir = FORWARD;
j3 0:f7e385400dec 469 rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 470 leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 471 }
j3 0:f7e385400dec 472 }
j3 0:f7e385400dec 473
j3 4:4144ef007743 474 if(samples < N)
j3 4:4144ef007743 475 {
j3 4:4144ef007743 476 accYaxisBuff[samples] = accData.yAxis.scaled;
j3 4:4144ef007743 477 accZaxisBuff[samples] = accData.zAxis.scaled;
j3 4:4144ef007743 478 gyroXaxisBuff[samples] = gyroData.xAxis.scaled;
j3 6:9712c04e13bf 479 pulseWidthBuff[samples] = static_cast<int32_t>(pulseWidth);
j3 4:4144ef007743 480 samples++;
j3 4:4144ef007743 481 }
j3 4:4144ef007743 482
j3 0:f7e385400dec 483 //save current error for next loop
j3 0:f7e385400dec 484 previousError = currentError;
j3 4:4144ef007743 485
j3 0:f7e385400dec 486 //Stop
j3 0:f7e385400dec 487 loopPulse = !loopPulse;
j3 0:f7e385400dec 488 }
j3 0:f7e385400dec 489 }
j3 3:7807a40b2459 490
j3 8:810396d34da8 491 if((pitch > 20.0F) || (pitch < -20.0F))
j3 4:4144ef007743 492 {
j3 4:4144ef007743 493 start = false;
j3 4:4144ef007743 494 }
j3 8:810396d34da8 495
j3 4:4144ef007743 496 pitch = 0.0F;
j3 4:4144ef007743 497 integral = 0.0F;
j3 4:4144ef007743 498 previousError = 0.0F;
j3 3:7807a40b2459 499 rightPwm.pulsewidth_us(0);
j3 3:7807a40b2459 500 leftPwm.pulsewidth_us(0);
CharlieO21 14:988b8d294259 501 rightDir.pulsewidth_us(0);
CharlieO21 14:988b8d294259 502 leftDir.pulsewidth_us(0);
j3 3:7807a40b2459 503
j3 3:7807a40b2459 504 rLED = LED_ON;
j3 3:7807a40b2459 505 gLED = LED_ON;
j3 3:7807a40b2459 506 wait(0.25);
j3 3:7807a40b2459 507 rLED = LED_OFF;
j3 3:7807a40b2459 508 gLED = LED_OFF;
j3 3:7807a40b2459 509 wait(0.25);
j3 3:7807a40b2459 510
j3 3:7807a40b2459 511 if(!uSDdetect && (samples > 0))
j3 4:4144ef007743 512 {
j3 4:4144ef007743 513 loopCoeffs[0] = setPoint;
j3 4:4144ef007743 514 bLED = LED_ON;
j3 4:4144ef007743 515 saveData(samples, loopCoeffs, gyroXaxisBuff, accYaxisBuff, accZaxisBuff, pulseWidthBuff);
j3 3:7807a40b2459 516 samples = 0;
j3 4:4144ef007743 517 bLED = LED_OFF;
j3 3:7807a40b2459 518 }
j3 0:f7e385400dec 519 }
j3 0:f7e385400dec 520 }
j3 0:f7e385400dec 521 else
j3 0:f7e385400dec 522 {
j3 0:f7e385400dec 523 while(1)
j3 0:f7e385400dec 524 {
j3 0:f7e385400dec 525 rLED = !rLED;
j3 0:f7e385400dec 526 wait(0.25);
j3 0:f7e385400dec 527 }
j3 0:f7e385400dec 528 }
j3 0:f7e385400dec 529 }
j3 4:4144ef007743 530
j3 4:4144ef007743 531
j3 4:4144ef007743 532 //*****************************************************************************
j3 4:4144ef007743 533 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX,
j3 6:9712c04e13bf 534 float * accY, float * accZ, int32_t * pw)
j3 4:4144ef007743 535 {
j3 4:4144ef007743 536 SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs
j3 4:4144ef007743 537 FILE *fp;
j3 4:4144ef007743 538
j3 4:4144ef007743 539 fp = fopen("/sd/balBot.txt", "a");
j3 4:4144ef007743 540 if(fp != NULL)
j3 4:4144ef007743 541 {
j3 8:810396d34da8 542 fprintf(fp, "Samples,%d,,,,,\n", numSamples);
j3 4:4144ef007743 543 fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]);
j3 4:4144ef007743 544 fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]);
j3 4:4144ef007743 545 fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]);
j3 4:4144ef007743 546 fprintf(fp, "KI, %f,,,,,\n", loopCoeffs[3]);
j3 4:4144ef007743 547 fprintf(fp, "KD, %f,,,,,\n", loopCoeffs[4]);
j3 4:4144ef007743 548 fprintf(fp, "DT, %f,,,,,\n", loopCoeffs[5]);
j3 4:4144ef007743 549 fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n");
j3 4:4144ef007743 550
j3 4:4144ef007743 551 float accPitch = 0.0F;
j3 5:f66e4666de56 552 float gyroDegrees = 0.0F;
j3 4:4144ef007743 553 float pitch = 0.0F;
j3 6:9712c04e13bf 554 float K = loopCoeffs[1];
j3 6:9712c04e13bf 555 float DT = loopCoeffs[5];
j3 4:4144ef007743 556
j3 4:4144ef007743 557 for(uint32_t idx = 0; idx < numSamples; idx++)
j3 4:4144ef007743 558 {
j3 6:9712c04e13bf 559 fprintf(fp, "%f,", idx*DT);
j3 4:4144ef007743 560 fprintf(fp, "%5.4f,", accY[idx]);
j3 4:4144ef007743 561 fprintf(fp, "%6.2f,", gyroX[idx]);
j3 6:9712c04e13bf 562 gyroDegrees += (gyroX[idx] * DT);
j3 5:f66e4666de56 563 fprintf(fp, "%5.2f,", gyroDegrees);
j3 4:4144ef007743 564 accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx]));
j3 4:4144ef007743 565 fprintf(fp, "%5.2f,", accPitch);
j3 6:9712c04e13bf 566 pitch = compFilter(K, pitch, gyroX[idx], accY[idx], accZ[idx], DT);
j3 5:f66e4666de56 567 fprintf(fp, "%5.2f,", pitch);
j3 6:9712c04e13bf 568 fprintf(fp, "%d", pw[idx]);
j3 4:4144ef007743 569 fprintf(fp, "\n");
j3 4:4144ef007743 570 }
j3 4:4144ef007743 571 fprintf(fp, "\n");
j3 4:4144ef007743 572 fclose(fp);
j3 4:4144ef007743 573 }
j3 4:4144ef007743 574 else
j3 4:4144ef007743 575 {
j3 4:4144ef007743 576 printf("Failed to open file\n");
j3 4:4144ef007743 577 }
j3 4:4144ef007743 578 }
j3 6:9712c04e13bf 579
j3 6:9712c04e13bf 580
j3 6:9712c04e13bf 581 //*****************************************************************************
j3 6:9712c04e13bf 582 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT)
j3 6:9712c04e13bf 583 {
j3 6:9712c04e13bf 584 return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
j3 6:9712c04e13bf 585 }