![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Balancing Bot using MAX32630FTHR
Dependencies: mbed BMI160 SDFileSystem max32630fthr
main.cpp
- Committer:
- Normanski
- Date:
- 2018-12-04
- Revision:
- 14:3ffd5961aa8e
- Parent:
- 12:0fb3a03aeecb
File content as of revision 14:3ffd5961aa8e:
/********************************************************************** * 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 * * The Balance Filter * http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf * */ #include "mbed.h" #include "max32630fthr.h" #include "bmi160.h" #include "SDFileSystem.h" float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT); void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, float * accY, float * accZ, int32_t * pw); //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() { start = !start; } //Setup callibrate button DigitalIn callibrateBtn(P5_2, PullUp); InterruptIn callibrate(P5_2); bool cal = false; void callibrateISR() { cal = !cal; } //Ping sensor trigger DigitalOut pingTrigger(P3_2, 0); Ticker pingTriggerTimer; void triggerPing() { pingTrigger = !pingTrigger; wait_us(1); pingTrigger = !pingTrigger; } DigitalIn p51(P5_1, PullNone); InterruptIn pingEchoRiseInt(P5_1); DigitalIn p45(P4_5, PullNone); InterruptIn pingEchoFallInt(P4_5); Timer pingTimer; float cm = 0; bool pingReady = false; const float US_PER_CM = 29.387; void echoStartISR() { pingTimer.reset(); } void echoStopISR() { uint32_t us = pingTimer.read_us()/2; cm = (us/ US_PER_CM); pingReady = true; } int main() { MAX32630FTHR pegasus; pegasus.init(MAX32630FTHR::VIO_3V3); /* * Do motor driver testing here. * P4_0 = FWD1(Logic_LvL) * P5_6 = REV1(Logic_LvL) * P5_5 = FWD2(Logic_Lvl) * P5_4 = REV2(Logic_Lvl) * Logic Table: * OpMode: FWD (M1=VDD, M2=GND) * - FWD# = 1 * - REV# = 0 * - bEN = 0 * OpMode: REV (M1=GND, M2=VDD) * - FWD# = 0 * - REV# = 1 * - bEN = 0 * OpMode: BREAK (M1=GND, M2=GND) * - FWD# = 0 * - REV# = 0 * - bEN = 0 */ DigitalOut FWD1(P4_0, 0); DigitalOut REV1(P5_6, 0); DigitalOut FWD2(P5_5, 0); DigitalOut REV2(P5_4, 0); FWD1 = 0; REV1 = 0; FWD2=0; REV2=0; while(1) { } /*************************************/ 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); uint32_t failures = 0; DigitalIn uSDdetect(P2_2, PullUp); static const uint32_t N = 14400; uint32_t samples = 0; float accYaxisBuff[N]; float accZaxisBuff[N]; float gyroXaxisBuff[N]; int32_t pulseWidthBuff[N]; //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); //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++; } wait(0.1); BMI160::AccConfig accConfig; BMI160::AccConfig accConfigRead; accConfig.range = BMI160::SENS_4G; accConfig.us = BMI160::ACC_US_OFF; accConfig.bwp = BMI160::ACC_BWP_2; accConfig.odr = BMI160::ACC_ODR_11; 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_11; 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++; } if(failures == 0) { printf("I got through! :)"); 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 = 0.9978F; float pitch = 0.0F; //PID coefficients //tunning with Wilson, Kc = 4.2, pc = 0.166 static const float DT = 0.00125F; static const float KP = 2.5F; static const float KI = (30.0F*DT); static const float KD = (0.05F/DT); float setPoint = 0.0F; float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT}; //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 INT1 to callback fx imuIntIn.rise(&imuISR); //Tie SW2 to callback fx startStop.fall(&startStopISR); //Tie P5_2 to callback fx callibrate.fall(&callibrateISR); bool firstCal = true; //Callbacks for echo measurement pingEchoRiseInt.fall(&echoStartISR); pingEchoFallInt.rise(&echoStopISR); //Timer for echo measurements pingTimer.start(); //Timer for trigger pingTriggerTimer.attach(&triggerPing, 0.05); //Position control vars/constants //static const float PING_SP = 20.0F; //static const float PING_KP = 0.0F; //float pingCurrentError = 0.0F; //control loop timming indicator DigitalOut loopPulse(P5_3, 0); //Count for averaging setPoint offset, 2 seconds of data uint32_t offsetCount = 0; while(1) { if(cal || (firstCal == true)) { cal = false; firstCal = false; pitch = 0.0F; setPoint = 0.0F; rLED = LED_ON; gLED = LED_ON; while(offsetCount < 1600) { if(drdy) { //Clear data ready flag drdy = false; //Read feedback sensors imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); //Feedback Block, pitch estimate in degrees pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT); //Accumalate pitch measurements setPoint += pitch; offsetCount++; } } //Average measurements setPoint = setPoint/offsetCount; printf("setPoint = %5.2f\n\n", setPoint); //Clear count for next time offsetCount = 0; } while(start && (pitch > -20.0F) && (pitch < 20.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, pitch estimate in degrees pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT); //PID Block //Error signal currentError = (setPoint - pitch); //Integral term, dt is included in KI 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)); /* if(pingReady) { //Get error signal pingReady = false; pingCurrentError = PING_SP - cm; } pulseWidth += (pingCurrentError * PING_KP); */ //Clamp to maximum duty cycle and check for forward/reverse //based on sign of error signal (negation of pitch since setPoint = 0) if(pulseWidth > MAX_PULSEWIDTH_US) { rightDir = FORWARD; leftDir = FORWARD; pulseWidth = 40.0F; rightPwm.pulsewidth_us(40); leftPwm.pulsewidth_us(40); } else if(pulseWidth < MIN_PULSEWIDTH_US) { rightDir = REVERSE; leftDir = REVERSE; pulseWidth = -40.0F; rightPwm.pulsewidth_us(40); leftPwm.pulsewidth_us(40); } else { if(pulseWidth < 0.0F) { rightDir = REVERSE; leftDir = REVERSE; rightPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth)); leftPwm.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth)); } else { rightDir = FORWARD; leftDir = FORWARD; rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth)); } } if(samples < N) { accYaxisBuff[samples] = accData.yAxis.scaled; accZaxisBuff[samples] = accData.zAxis.scaled; gyroXaxisBuff[samples] = gyroData.xAxis.scaled; pulseWidthBuff[samples] = static_cast<int32_t>(pulseWidth); samples++; } //save current error for next loop previousError = currentError; //Stop loopPulse = !loopPulse; } } if((pitch > 20.0F) || (pitch < -20.0F)) { start = false; } pitch = 0.0F; integral = 0.0F; previousError = 0.0F; rightPwm.pulsewidth_us(0); leftPwm.pulsewidth_us(0); rLED = LED_ON; gLED = LED_ON; wait(0.25); rLED = LED_OFF; gLED = LED_OFF; wait(0.25); if(!uSDdetect && (samples > 0)) { loopCoeffs[0] = setPoint; bLED = LED_ON; saveData(samples, loopCoeffs, gyroXaxisBuff, accYaxisBuff, accZaxisBuff, pulseWidthBuff); samples = 0; bLED = LED_OFF; } } } else { while(1) { printf("I'M STUCKED!"); rLED = !rLED; wait(0.25); } } } //***************************************************************************** void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, float * accY, float * accZ, int32_t * pw) { SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs FILE *fp; fp = fopen("/sd/balBot.txt", "a"); if(fp != NULL) { fprintf(fp, "Samples,%d,,,,,\n", numSamples); fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]); fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]); fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]); fprintf(fp, "KI, %f,,,,,\n", loopCoeffs[3]); fprintf(fp, "KD, %f,,,,,\n", loopCoeffs[4]); fprintf(fp, "DT, %f,,,,,\n", loopCoeffs[5]); fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n"); float accPitch = 0.0F; float gyroDegrees = 0.0F; float pitch = 0.0F; float K = loopCoeffs[1]; float DT = loopCoeffs[5]; for(uint32_t idx = 0; idx < numSamples; idx++) { fprintf(fp, "%f,", idx*DT); fprintf(fp, "%5.4f,", accY[idx]); fprintf(fp, "%6.2f,", gyroX[idx]); gyroDegrees += (gyroX[idx] * DT); fprintf(fp, "%5.2f,", gyroDegrees); accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx])); fprintf(fp, "%5.2f,", accPitch); pitch = compFilter(K, pitch, gyroX[idx], accY[idx], accZ[idx], DT); fprintf(fp, "%5.2f,", pitch); fprintf(fp, "%d", pw[idx]); fprintf(fp, "\n"); } fprintf(fp, "\n"); fclose(fp); } else { printf("Failed to open file\n"); } } //***************************************************************************** float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT) { return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ)))); }