Carlos Orendain / Mbed 2 deprecated SELF_BALANCING_BOT

Self-Balancing bot for MAX32630 and two wheel car.

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

main.cpp@3:7807a40b2459, 2017-01-09 (annotated)

Committer:
j3
Date:
Mon Jan 09 19:08:30 2017 +0000
Revision:
3:7807a40b2459
Parent:
2:5af1d818331f
Child:
4:4144ef007743
Added SD Card data logging

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