![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Self-Balancing bot for MAX32630 and two wheel car.
Dependencies: mbed BMI160 SDFileSystem max32630fthr
Diff: main.cpp
- Revision:
- 0:f7e385400dec
- Child:
- 1:e9dd53326ba1
diff -r 000000000000 -r f7e385400dec main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jan 06 03:16:04 2017 +0000 @@ -0,0 +1,372 @@ +/********************************************************************** +* Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved. +* +* Permission is hereby granted, free of charge, to any person obtaining a +* copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation +* the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included +* in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +* IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES +* OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +* OTHER DEALINGS IN THE SOFTWARE. +* +* Except as contained in this notice, the name of Maxim Integrated +* Products, Inc. shall not be used except as stated in the Maxim Integrated +* Products, Inc. Branding Policy. +* +* The mere transfer of this software does not imply any licenses +* of trade secrets, proprietary technology, copyrights, patents, +* trademarks, maskwork rights, or any other form of intellectual +* property whatsoever. Maxim Integrated Products, Inc. retains all +* ownership rights. +**********************************************************************/ + + +/*References +* +* Circuit Cellar Issue 316 November 2016 +* BalanceBot: A Self-Balancing, Two-Wheeled Robot +* +* Reading a IMU Without Kalman: The Complementary Filter +* http://www.pieter-jan.com/node/11 +* +*/ + + +#include "mbed.h" +#include "max32630fthr.h" +#include "bmi160.h" + + +//Setup interrupt in from imu +InterruptIn imuIntIn(P3_6); +bool drdy = false; +void imuISR() +{ + drdy = true; +} + + +//Setup start/stop button +DigitalIn startStopBtn(P2_3, PullUp); +InterruptIn startStop(P2_3); +bool start = false; +void startStopISR() +{ + if(start) + { + start = false; + } + else + { + start = true; + } +} + + +int main() +{ + MAX32630FTHR pegasus; + pegasus.init(MAX32630FTHR::VIO_3V3); + + static const float MAX_PULSEWIDTH_US = 40.0F; + static const float MIN_PULSEWIDTH_US = -40.0F; + + static const bool FORWARD = 0; + static const bool REVERSE = 1; + + //Setup left motor(from driver seat) + DigitalOut leftDir(P5_6, FORWARD); + PwmOut leftPwm(P4_0); + leftPwm.period_us(40); + leftPwm.pulsewidth_us(0); + + //Make sure P4_2 and P4_3 are Hi-Z + DigitalIn p42_hiZ(P4_2, PullNone); + DigitalIn p43_hiZ(P4_3, PullNone); + + //Setup right motor(from driver seat) + DigitalOut rightDir(P5_4, FORWARD); + PwmOut rightPwm(P5_5); + rightPwm.period_us(40); + rightPwm.pulsewidth_us(0); + + //Turn off RGB LEDs + DigitalOut rLED(LED1, LED_OFF); + DigitalOut gLED(LED2, LED_OFF); + DigitalOut bLED(LED3, LED_OFF); + + //Setup I2C bus for IMU + I2C i2cBus(P5_7, P6_0); + i2cBus.frequency(400000); + + //Get IMU instance and configure it + BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); + + uint32_t failures = 0; + + BMI160::AccConfig accConfig; + BMI160::AccConfig accConfigRead; + accConfig.range = BMI160::SENS_2G; + accConfig.us = BMI160::ACC_US_OFF; + accConfig.bwp = BMI160::ACC_BWP_2; + accConfig.odr = BMI160::ACC_ODR_12; + if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) + { + if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR) + { + if((accConfig.range != accConfigRead.range) || + (accConfig.us != accConfigRead.us) || + (accConfig.bwp != accConfigRead.bwp) || + (accConfig.odr != accConfigRead.odr)) + { + printf("ACC read data desn't equal set data\n\n"); + printf("ACC Set Range = %d\n", accConfig.range); + printf("ACC Set UnderSampling = %d\n", accConfig.us); + printf("ACC Set BandWidthParam = %d\n", accConfig.bwp); + printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr); + printf("ACC Read Range = %d\n", accConfigRead.range); + printf("ACC Read UnderSampling = %d\n", accConfigRead.us); + printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp); + printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr); + failures++; + } + + } + else + { + printf("Failed to read back accelerometer configuration\n"); + failures++; + } + } + else + { + printf("Failed to set accelerometer configuration\n"); + failures++; + } + + BMI160::GyroConfig gyroConfig; + BMI160::GyroConfig gyroConfigRead; + gyroConfig.range = BMI160::DPS_2000; + gyroConfig.bwp = BMI160::GYRO_BWP_2; + gyroConfig.odr = BMI160::GYRO_ODR_12; + if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) + { + if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR) + { + if((gyroConfig.range != gyroConfigRead.range) || + (gyroConfig.bwp != gyroConfigRead.bwp) || + (gyroConfig.odr != gyroConfigRead.odr)) + { + printf("GYRO read data desn't equal set data\n\n"); + printf("GYRO Set Range = %d\n", gyroConfig.range); + printf("GYRO Set BandWidthParam = %d\n", gyroConfig.bwp); + printf("GYRO Set OutputDataRate = %d\n\n", gyroConfig.odr); + printf("GYRO Read Range = %d\n", gyroConfigRead.range); + printf("GYRO Read BandWidthParam = %d\n", gyroConfigRead.bwp); + printf("GYRO Read OutputDataRate = %d\n\n", gyroConfigRead.odr); + failures++; + } + + } + else + { + printf("Failed to read back gyroscope configuration\n"); + failures++; + } + } + else + { + printf("Failed to set gyroscope configuration\n"); + failures++; + } + + //Power up sensors in normal mode + if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + printf("Failed to set gyroscope power mode\n"); + failures++; + } + + wait(0.1); + + if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + printf("Failed to set accelerometer power mode\n"); + failures++; + } + + if(failures == 0) + { + printf("ACC Range = %d\n", accConfig.range); + printf("ACC UnderSampling = %d\n", accConfig.us); + printf("ACC BandWidthParam = %d\n", accConfig.bwp); + printf("ACC OutputDataRate = %d\n\n", accConfig.odr); + printf("GYRO Range = %d\n", gyroConfig.range); + printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp); + printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr); + wait(1.0); + + //Sensor data vars + BMI160::SensorData accData; + BMI160::SensorData gyroData; + BMI160::SensorTime sensorTime; + + //Complementary Filter vars, filter weight K + static const float K = .996F; + float thetaGyro = 0.0F; + float thetaAcc = 0.0F; + float pitch = 0.0F; + + //PID coefficients + static const float DT = 0.000625F; + static const float KP = 2.0F; + static const float KI = (2.0F * DT); + static const float KD = (0.025F / DT); + static const float SP = 1.8F; + + //Control loop vars + float currentError, previousError; + float integral = 0.0F; + float derivative = 0.0F; + float pulseWidth; + + //Enable data ready interrupt from imu for constant loop timming + imu.writeRegister(BMI160::INT_EN_1, 0x10); + //Active High PushPull output on INT1 + imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A); + //Map data ready interrupt to INT1 + imu.writeRegister(BMI160::INT_MAP_1, 0x80); + //Tie input to callback fx + imuIntIn.rise(&imuISR); + + startStop.fall(&startStopISR); + + //control loop timming indicator + DigitalOut loopPulse(P5_3, 0); + + while(1) + { + rLED = LED_ON; + gLED = LED_OFF; + rightPwm.pulsewidth_us(0); + leftPwm.pulsewidth_us(0); + + while(start && (pitch > -45.0F) && (pitch < 45.0F)) + { + rLED = LED_OFF; + gLED = LED_ON; + if(drdy) + { + //Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus + //At 1600Hz ODR, ~73% of loop time is active doing something + loopPulse = !loopPulse; + + //Clear data ready flag + drdy = false; + + //Read feedback sensors + imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); + + //Feedback Block, Complementary filter + //Prediction estimate + thetaGyro = (pitch + (gyroData.xAxis.scaled * DT)); + //Measurement estimate + thetaAcc = ((180.0F/3.14F) * atan(accData.yAxis.scaled/accData.zAxis.scaled)); + //Feedback, Pitch estimate in degrees + pitch = ((K * thetaGyro) + ((1.0F - K) * thetaAcc)); + + //PID Block + //Error signal + currentError = (SP - pitch); + + //Integral term, dt is included in KI + //If statement addresses integral windup + if(currentError == 0.0F) + { + integral = 0.0F; + } + else if(((currentError < 0.0F) && (previousError > 0.0F)) || + ((currentError > 0.0F) && (previousError < 0.0F))) + { + integral = 0.0F; + } + else + { + integral = (integral + currentError); + } + + //Derivative term, dt is included in KD + derivative = (currentError - previousError); + + //Set right/left pulsewidth + //Just balance for now, so both left/right are the same + pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative)); + + //printf("Pulsewidth = %f\n\n", pulseWidth); + + //Clamp to maximum duty cycle and check for forward/reverse + //based on sign of error signal (negation of pitch since SP = 0) + if(pulseWidth > MAX_PULSEWIDTH_US) + { + rightDir = FORWARD; + leftDir = FORWARD; + rightPwm.pulsewidth_us(40); + leftPwm.pulsewidth_us(40); + } + else if(pulseWidth < MIN_PULSEWIDTH_US) + { + rightDir = REVERSE; + leftDir = REVERSE; + rightPwm.pulsewidth_us(40); + leftPwm.pulsewidth_us(40); + } + else + { + if(pulseWidth < 0.0F) + { + rightDir = REVERSE; + leftDir = REVERSE; + pulseWidth = (1 - pulseWidth); + rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); + leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); + } + else + { + rightDir = FORWARD; + leftDir = FORWARD; + rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); + leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); + } + } + + //save current error for next loop + previousError = currentError; + + //Stop + loopPulse = !loopPulse; + } + } + + start = false; + pitch = 0.0; + } + } + else + { + while(1) + { + rLED = !rLED; + wait(0.25); + } + } +}