Carlos Orendain / Mbed 2 deprecated SELF_BALANCING_BOT

Self-Balancing bot for MAX32630 and two wheel car.

Dependencies:   mbed BMI160 SDFileSystem max32630fthr

main.cpp@0:f7e385400dec, 2017-01-06 (annotated)

Committer:
j3
Date:
Fri Jan 06 03:16:04 2017 +0000
Revision:
0:f7e385400dec
Child:
1:e9dd53326ba1
Balanced, still needs fine tunning

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 0:f7e385400dec 48
j3 0:f7e385400dec 49
j3 0:f7e385400dec 50 //Setup interrupt in from imu
j3 0:f7e385400dec 51 InterruptIn imuIntIn(P3_6);
j3 0:f7e385400dec 52 bool drdy = false;
j3 0:f7e385400dec 53 void imuISR()
j3 0:f7e385400dec 54 {
j3 0:f7e385400dec 55 drdy = true;
j3 0:f7e385400dec 56 }
j3 0:f7e385400dec 57
j3 0:f7e385400dec 58
j3 0:f7e385400dec 59 //Setup start/stop button
j3 0:f7e385400dec 60 DigitalIn startStopBtn(P2_3, PullUp);
j3 0:f7e385400dec 61 InterruptIn startStop(P2_3);
j3 0:f7e385400dec 62 bool start = false;
j3 0:f7e385400dec 63 void startStopISR()
j3 0:f7e385400dec 64 {
j3 0:f7e385400dec 65 if(start)
j3 0:f7e385400dec 66 {
j3 0:f7e385400dec 67 start = false;
j3 0:f7e385400dec 68 }
j3 0:f7e385400dec 69 else
j3 0:f7e385400dec 70 {
j3 0:f7e385400dec 71 start = true;
j3 0:f7e385400dec 72 }
j3 0:f7e385400dec 73 }
j3 0:f7e385400dec 74
j3 0:f7e385400dec 75
j3 0:f7e385400dec 76 int main()
j3 0:f7e385400dec 77 {
j3 0:f7e385400dec 78 MAX32630FTHR pegasus;
j3 0:f7e385400dec 79 pegasus.init(MAX32630FTHR::VIO_3V3);
j3 0:f7e385400dec 80
j3 0:f7e385400dec 81 static const float MAX_PULSEWIDTH_US = 40.0F;
j3 0:f7e385400dec 82 static const float MIN_PULSEWIDTH_US = -40.0F;
j3 0:f7e385400dec 83
j3 0:f7e385400dec 84 static const bool FORWARD = 0;
j3 0:f7e385400dec 85 static const bool REVERSE = 1;
j3 0:f7e385400dec 86
j3 0:f7e385400dec 87 //Setup left motor(from driver seat)
j3 0:f7e385400dec 88 DigitalOut leftDir(P5_6, FORWARD);
j3 0:f7e385400dec 89 PwmOut leftPwm(P4_0);
j3 0:f7e385400dec 90 leftPwm.period_us(40);
j3 0:f7e385400dec 91 leftPwm.pulsewidth_us(0);
j3 0:f7e385400dec 92
j3 0:f7e385400dec 93 //Make sure P4_2 and P4_3 are Hi-Z
j3 0:f7e385400dec 94 DigitalIn p42_hiZ(P4_2, PullNone);
j3 0:f7e385400dec 95 DigitalIn p43_hiZ(P4_3, PullNone);
j3 0:f7e385400dec 96
j3 0:f7e385400dec 97 //Setup right motor(from driver seat)
j3 0:f7e385400dec 98 DigitalOut rightDir(P5_4, FORWARD);
j3 0:f7e385400dec 99 PwmOut rightPwm(P5_5);
j3 0:f7e385400dec 100 rightPwm.period_us(40);
j3 0:f7e385400dec 101 rightPwm.pulsewidth_us(0);
j3 0:f7e385400dec 102
j3 0:f7e385400dec 103 //Turn off RGB LEDs
j3 0:f7e385400dec 104 DigitalOut rLED(LED1, LED_OFF);
j3 0:f7e385400dec 105 DigitalOut gLED(LED2, LED_OFF);
j3 0:f7e385400dec 106 DigitalOut bLED(LED3, LED_OFF);
j3 0:f7e385400dec 107
j3 0:f7e385400dec 108 //Setup I2C bus for IMU
j3 0:f7e385400dec 109 I2C i2cBus(P5_7, P6_0);
j3 0:f7e385400dec 110 i2cBus.frequency(400000);
j3 0:f7e385400dec 111
j3 0:f7e385400dec 112 //Get IMU instance and configure it
j3 0:f7e385400dec 113 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
j3 0:f7e385400dec 114
j3 0:f7e385400dec 115 uint32_t failures = 0;
j3 0:f7e385400dec 116
j3 0:f7e385400dec 117 BMI160::AccConfig accConfig;
j3 0:f7e385400dec 118 BMI160::AccConfig accConfigRead;
j3 0:f7e385400dec 119 accConfig.range = BMI160::SENS_2G;
j3 0:f7e385400dec 120 accConfig.us = BMI160::ACC_US_OFF;
j3 0:f7e385400dec 121 accConfig.bwp = BMI160::ACC_BWP_2;
j3 0:f7e385400dec 122 accConfig.odr = BMI160::ACC_ODR_12;
j3 0:f7e385400dec 123 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 124 {
j3 0:f7e385400dec 125 if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 126 {
j3 0:f7e385400dec 127 if((accConfig.range != accConfigRead.range) ||
j3 0:f7e385400dec 128 (accConfig.us != accConfigRead.us) ||
j3 0:f7e385400dec 129 (accConfig.bwp != accConfigRead.bwp) ||
j3 0:f7e385400dec 130 (accConfig.odr != accConfigRead.odr))
j3 0:f7e385400dec 131 {
j3 0:f7e385400dec 132 printf("ACC read data desn't equal set data\n\n");
j3 0:f7e385400dec 133 printf("ACC Set Range = %d\n", accConfig.range);
j3 0:f7e385400dec 134 printf("ACC Set UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 135 printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 136 printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 137 printf("ACC Read Range = %d\n", accConfigRead.range);
j3 0:f7e385400dec 138 printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
j3 0:f7e385400dec 139 printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
j3 0:f7e385400dec 140 printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
j3 0:f7e385400dec 141 failures++;
j3 0:f7e385400dec 142 }
j3 0:f7e385400dec 143
j3 0:f7e385400dec 144 }
j3 0:f7e385400dec 145 else
j3 0:f7e385400dec 146 {
j3 0:f7e385400dec 147 printf("Failed to read back accelerometer configuration\n");
j3 0:f7e385400dec 148 failures++;
j3 0:f7e385400dec 149 }
j3 0:f7e385400dec 150 }
j3 0:f7e385400dec 151 else
j3 0:f7e385400dec 152 {
j3 0:f7e385400dec 153 printf("Failed to set accelerometer configuration\n");
j3 0:f7e385400dec 154 failures++;
j3 0:f7e385400dec 155 }
j3 0:f7e385400dec 156
j3 0:f7e385400dec 157 BMI160::GyroConfig gyroConfig;
j3 0:f7e385400dec 158 BMI160::GyroConfig gyroConfigRead;
j3 0:f7e385400dec 159 gyroConfig.range = BMI160::DPS_2000;
j3 0:f7e385400dec 160 gyroConfig.bwp = BMI160::GYRO_BWP_2;
j3 0:f7e385400dec 161 gyroConfig.odr = BMI160::GYRO_ODR_12;
j3 0:f7e385400dec 162 if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 163 {
j3 0:f7e385400dec 164 if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 165 {
j3 0:f7e385400dec 166 if((gyroConfig.range != gyroConfigRead.range) ||
j3 0:f7e385400dec 167 (gyroConfig.bwp != gyroConfigRead.bwp) ||
j3 0:f7e385400dec 168 (gyroConfig.odr != gyroConfigRead.odr))
j3 0:f7e385400dec 169 {
j3 0:f7e385400dec 170 printf("GYRO read data desn't equal set data\n\n");
j3 0:f7e385400dec 171 printf("GYRO Set Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 172 printf("GYRO Set BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 173 printf("GYRO Set OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 174 printf("GYRO Read Range = %d\n", gyroConfigRead.range);
j3 0:f7e385400dec 175 printf("GYRO Read BandWidthParam = %d\n", gyroConfigRead.bwp);
j3 0:f7e385400dec 176 printf("GYRO Read OutputDataRate = %d\n\n", gyroConfigRead.odr);
j3 0:f7e385400dec 177 failures++;
j3 0:f7e385400dec 178 }
j3 0:f7e385400dec 179
j3 0:f7e385400dec 180 }
j3 0:f7e385400dec 181 else
j3 0:f7e385400dec 182 {
j3 0:f7e385400dec 183 printf("Failed to read back gyroscope configuration\n");
j3 0:f7e385400dec 184 failures++;
j3 0:f7e385400dec 185 }
j3 0:f7e385400dec 186 }
j3 0:f7e385400dec 187 else
j3 0:f7e385400dec 188 {
j3 0:f7e385400dec 189 printf("Failed to set gyroscope configuration\n");
j3 0:f7e385400dec 190 failures++;
j3 0:f7e385400dec 191 }
j3 0:f7e385400dec 192
j3 0:f7e385400dec 193 //Power up sensors in normal mode
j3 0:f7e385400dec 194 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 195 {
j3 0:f7e385400dec 196 printf("Failed to set gyroscope power mode\n");
j3 0:f7e385400dec 197 failures++;
j3 0:f7e385400dec 198 }
j3 0:f7e385400dec 199
j3 0:f7e385400dec 200 wait(0.1);
j3 0:f7e385400dec 201
j3 0:f7e385400dec 202 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
j3 0:f7e385400dec 203 {
j3 0:f7e385400dec 204 printf("Failed to set accelerometer power mode\n");
j3 0:f7e385400dec 205 failures++;
j3 0:f7e385400dec 206 }
j3 0:f7e385400dec 207
j3 0:f7e385400dec 208 if(failures == 0)
j3 0:f7e385400dec 209 {
j3 0:f7e385400dec 210 printf("ACC Range = %d\n", accConfig.range);
j3 0:f7e385400dec 211 printf("ACC UnderSampling = %d\n", accConfig.us);
j3 0:f7e385400dec 212 printf("ACC BandWidthParam = %d\n", accConfig.bwp);
j3 0:f7e385400dec 213 printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
j3 0:f7e385400dec 214 printf("GYRO Range = %d\n", gyroConfig.range);
j3 0:f7e385400dec 215 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
j3 0:f7e385400dec 216 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
j3 0:f7e385400dec 217 wait(1.0);
j3 0:f7e385400dec 218
j3 0:f7e385400dec 219 //Sensor data vars
j3 0:f7e385400dec 220 BMI160::SensorData accData;
j3 0:f7e385400dec 221 BMI160::SensorData gyroData;
j3 0:f7e385400dec 222 BMI160::SensorTime sensorTime;
j3 0:f7e385400dec 223
j3 0:f7e385400dec 224 //Complementary Filter vars, filter weight K
j3 0:f7e385400dec 225 static const float K = .996F;
j3 0:f7e385400dec 226 float thetaGyro = 0.0F;
j3 0:f7e385400dec 227 float thetaAcc = 0.0F;
j3 0:f7e385400dec 228 float pitch = 0.0F;
j3 0:f7e385400dec 229
j3 0:f7e385400dec 230 //PID coefficients
j3 0:f7e385400dec 231 static const float DT = 0.000625F;
j3 0:f7e385400dec 232 static const float KP = 2.0F;
j3 0:f7e385400dec 233 static const float KI = (2.0F * DT);
j3 0:f7e385400dec 234 static const float KD = (0.025F / DT);
j3 0:f7e385400dec 235 static const float SP = 1.8F;
j3 0:f7e385400dec 236
j3 0:f7e385400dec 237 //Control loop vars
j3 0:f7e385400dec 238 float currentError, previousError;
j3 0:f7e385400dec 239 float integral = 0.0F;
j3 0:f7e385400dec 240 float derivative = 0.0F;
j3 0:f7e385400dec 241 float pulseWidth;
j3 0:f7e385400dec 242
j3 0:f7e385400dec 243 //Enable data ready interrupt from imu for constant loop timming
j3 0:f7e385400dec 244 imu.writeRegister(BMI160::INT_EN_1, 0x10);
j3 0:f7e385400dec 245 //Active High PushPull output on INT1
j3 0:f7e385400dec 246 imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A);
j3 0:f7e385400dec 247 //Map data ready interrupt to INT1
j3 0:f7e385400dec 248 imu.writeRegister(BMI160::INT_MAP_1, 0x80);
j3 0:f7e385400dec 249 //Tie input to callback fx
j3 0:f7e385400dec 250 imuIntIn.rise(&imuISR);
j3 0:f7e385400dec 251
j3 0:f7e385400dec 252 startStop.fall(&startStopISR);
j3 0:f7e385400dec 253
j3 0:f7e385400dec 254 //control loop timming indicator
j3 0:f7e385400dec 255 DigitalOut loopPulse(P5_3, 0);
j3 0:f7e385400dec 256
j3 0:f7e385400dec 257 while(1)
j3 0:f7e385400dec 258 {
j3 0:f7e385400dec 259 rLED = LED_ON;
j3 0:f7e385400dec 260 gLED = LED_OFF;
j3 0:f7e385400dec 261 rightPwm.pulsewidth_us(0);
j3 0:f7e385400dec 262 leftPwm.pulsewidth_us(0);
j3 0:f7e385400dec 263
j3 0:f7e385400dec 264 while(start && (pitch > -45.0F) && (pitch < 45.0F))
j3 0:f7e385400dec 265 {
j3 0:f7e385400dec 266 rLED = LED_OFF;
j3 0:f7e385400dec 267 gLED = LED_ON;
j3 0:f7e385400dec 268 if(drdy)
j3 0:f7e385400dec 269 {
j3 0:f7e385400dec 270 //Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus
j3 0:f7e385400dec 271 //At 1600Hz ODR, ~73% of loop time is active doing something
j3 0:f7e385400dec 272 loopPulse = !loopPulse;
j3 0:f7e385400dec 273
j3 0:f7e385400dec 274 //Clear data ready flag
j3 0:f7e385400dec 275 drdy = false;
j3 0:f7e385400dec 276
j3 0:f7e385400dec 277 //Read feedback sensors
j3 0:f7e385400dec 278 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
j3 0:f7e385400dec 279
j3 0:f7e385400dec 280 //Feedback Block, Complementary filter
j3 0:f7e385400dec 281 //Prediction estimate
j3 0:f7e385400dec 282 thetaGyro = (pitch + (gyroData.xAxis.scaled * DT));
j3 0:f7e385400dec 283 //Measurement estimate
j3 0:f7e385400dec 284 thetaAcc = ((180.0F/3.14F) * atan(accData.yAxis.scaled/accData.zAxis.scaled));
j3 0:f7e385400dec 285 //Feedback, Pitch estimate in degrees
j3 0:f7e385400dec 286 pitch = ((K * thetaGyro) + ((1.0F - K) * thetaAcc));
j3 0:f7e385400dec 287
j3 0:f7e385400dec 288 //PID Block
j3 0:f7e385400dec 289 //Error signal
j3 0:f7e385400dec 290 currentError = (SP - pitch);
j3 0:f7e385400dec 291
j3 0:f7e385400dec 292 //Integral term, dt is included in KI
j3 0:f7e385400dec 293 //If statement addresses integral windup
j3 0:f7e385400dec 294 if(currentError == 0.0F)
j3 0:f7e385400dec 295 {
j3 0:f7e385400dec 296 integral = 0.0F;
j3 0:f7e385400dec 297 }
j3 0:f7e385400dec 298 else if(((currentError < 0.0F) && (previousError > 0.0F)) ||
j3 0:f7e385400dec 299 ((currentError > 0.0F) && (previousError < 0.0F)))
j3 0:f7e385400dec 300 {
j3 0:f7e385400dec 301 integral = 0.0F;
j3 0:f7e385400dec 302 }
j3 0:f7e385400dec 303 else
j3 0:f7e385400dec 304 {
j3 0:f7e385400dec 305 integral = (integral + currentError);
j3 0:f7e385400dec 306 }
j3 0:f7e385400dec 307
j3 0:f7e385400dec 308 //Derivative term, dt is included in KD
j3 0:f7e385400dec 309 derivative = (currentError - previousError);
j3 0:f7e385400dec 310
j3 0:f7e385400dec 311 //Set right/left pulsewidth
j3 0:f7e385400dec 312 //Just balance for now, so both left/right are the same
j3 0:f7e385400dec 313 pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative));
j3 0:f7e385400dec 314
j3 0:f7e385400dec 315 //printf("Pulsewidth = %f\n\n", pulseWidth);
j3 0:f7e385400dec 316
j3 0:f7e385400dec 317 //Clamp to maximum duty cycle and check for forward/reverse
j3 0:f7e385400dec 318 //based on sign of error signal (negation of pitch since SP = 0)
j3 0:f7e385400dec 319 if(pulseWidth > MAX_PULSEWIDTH_US)
j3 0:f7e385400dec 320 {
j3 0:f7e385400dec 321 rightDir = FORWARD;
j3 0:f7e385400dec 322 leftDir = FORWARD;
j3 0:f7e385400dec 323 rightPwm.pulsewidth_us(40);
j3 0:f7e385400dec 324 leftPwm.pulsewidth_us(40);
j3 0:f7e385400dec 325 }
j3 0:f7e385400dec 326 else if(pulseWidth < MIN_PULSEWIDTH_US)
j3 0:f7e385400dec 327 {
j3 0:f7e385400dec 328 rightDir = REVERSE;
j3 0:f7e385400dec 329 leftDir = REVERSE;
j3 0:f7e385400dec 330 rightPwm.pulsewidth_us(40);
j3 0:f7e385400dec 331 leftPwm.pulsewidth_us(40);
j3 0:f7e385400dec 332 }
j3 0:f7e385400dec 333 else
j3 0:f7e385400dec 334 {
j3 0:f7e385400dec 335 if(pulseWidth < 0.0F)
j3 0:f7e385400dec 336 {
j3 0:f7e385400dec 337 rightDir = REVERSE;
j3 0:f7e385400dec 338 leftDir = REVERSE;
j3 0:f7e385400dec 339 pulseWidth = (1 - pulseWidth);
j3 0:f7e385400dec 340 rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 341 leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 342 }
j3 0:f7e385400dec 343 else
j3 0:f7e385400dec 344 {
j3 0:f7e385400dec 345 rightDir = FORWARD;
j3 0:f7e385400dec 346 leftDir = FORWARD;
j3 0:f7e385400dec 347 rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 348 leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
j3 0:f7e385400dec 349 }
j3 0:f7e385400dec 350 }
j3 0:f7e385400dec 351
j3 0:f7e385400dec 352 //save current error for next loop
j3 0:f7e385400dec 353 previousError = currentError;
j3 0:f7e385400dec 354
j3 0:f7e385400dec 355 //Stop
j3 0:f7e385400dec 356 loopPulse = !loopPulse;
j3 0:f7e385400dec 357 }
j3 0:f7e385400dec 358 }
j3 0:f7e385400dec 359
j3 0:f7e385400dec 360 start = false;
j3 0:f7e385400dec 361 pitch = 0.0;
j3 0:f7e385400dec 362 }
j3 0:f7e385400dec 363 }
j3 0:f7e385400dec 364 else
j3 0:f7e385400dec 365 {
j3 0:f7e385400dec 366 while(1)
j3 0:f7e385400dec 367 {
j3 0:f7e385400dec 368 rLED = !rLED;
j3 0:f7e385400dec 369 wait(0.25);
j3 0:f7e385400dec 370 }
j3 0:f7e385400dec 371 }
j3 0:f7e385400dec 372 }