TSDA Robotics / Mbed 2 deprecated MAX32630FTHR_BALANCE_BOT

Balancing Bot using MAX32630FTHR

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

main.cpp@14:3ffd5961aa8e, 2018-12-04 (annotated)

Committer:
Normanski
Date:
Tue Dec 04 09:49:00 2018 +0000
Revision:
14:3ffd5961aa8e
Parent:
12:0fb3a03aeecb 
Initial

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