Self-Balancing bot for MAX32630 and two wheel car.

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

Committer:
j3
Date:
Fri Jan 13 07:29:19 2017 +0000
Revision:
5:f66e4666de56
Parent:
4:4144ef007743
Child:
6:9712c04e13bf
Fixed comp filter

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