Justin Jordan / Mbed 2 deprecated MAX32630FTHR_BALANCE_BOT

Balancing Bot using MAX32630FTHR

Dependencies:   BMI160 max32630fthr mbed SDFileSystem

main.cpp@13:40631fdbece2, 2017-02-06 (annotated)

Committer:
j3
Date:
Mon Feb 06 20:38:25 2017 +0000
Revision:
13:40631fdbece2
Parent:
12:0fb3a03aeecb 
Switched to official mbed lib

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