main.cpp

00001 /**********************************************************************
00002 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
00003 *
00004 * Permission is hereby granted, free of charge, to any person obtaining a
00005 * copy of this software and associated documentation files (the "Software"),
00006 * to deal in the Software without restriction, including without limitation
00007 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00008 * and/or sell copies of the Software, and to permit persons to whom the
00009 * Software is furnished to do so, subject to the following conditions:
00010 *
00011 * The above copyright notice and this permission notice shall be included
00012 * in all copies or substantial portions of the Software.
00013 *
00014 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00018 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00019 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00020 * OTHER DEALINGS IN THE SOFTWARE.
00021 *
00022 * Except as contained in this notice, the name of Maxim Integrated
00023 * Products, Inc. shall not be used except as stated in the Maxim Integrated
00024 * Products, Inc. Branding Policy.
00025 *
00026 * The mere transfer of this software does not imply any licenses
00027 * of trade secrets, proprietary technology, copyrights, patents,
00028 * trademarks, maskwork rights, or any other form of intellectual
00029 * property whatsoever. Maxim Integrated Products, Inc. retains all
00030 * ownership rights.
00031 **********************************************************************/
00032 
00033 
00034 /*References
00035 *
00036 * Circuit Cellar Issue 316 November 2016
00037 * BalanceBot: A Self-Balancing, Two-Wheeled Robot
00038 *
00039 * Reading a IMU Without Kalman: The Complementary Filter
00040 * http://www.pieter-jan.com/node/11
00041 *
00042 * The Balance Filter
00043 * http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf
00044 *
00045 */
00046 
00047 
00048 #include "mbed.h"
00049 #include "max32630fthr.h"
00050 #include "bmi160.h"
00051 #include "SDFileSystem.h"
00052 
00053 
00054 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT);
00055                  
00056 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, 
00057               float * accY, float * accZ, int32_t * pw);
00058 
00059 
00060 //Setup interrupt in from imu
00061 InterruptIn imuIntIn(P3_6);
00062 bool drdy = false;
00063 void imuISR()
00064 {
00065     drdy = true;
00066 }
00067 
00068 
00069 //Setup start/stop button
00070 DigitalIn startStopBtn(P2_3, PullUp);
00071 InterruptIn startStop(P2_3);
00072 bool start = false;
00073 void startStopISR()
00074 {
00075     start = !start;
00076 }
00077 
00078 
00079 //Setup callibrate button
00080 DigitalIn callibrateBtn(P5_2, PullUp);
00081 InterruptIn callibrate(P5_2);
00082 bool cal = false;
00083 void callibrateISR()
00084 {
00085     cal = !cal;
00086 }
00087 
00088 
00089 //Ping sensor trigger
00090 DigitalOut pingTrigger(P3_2, 0);
00091 Ticker pingTriggerTimer;
00092 
00093 void triggerPing()
00094 {
00095     pingTrigger = !pingTrigger;
00096     wait_us(1);
00097     pingTrigger = !pingTrigger;
00098 }
00099 
00100 DigitalIn p51(P5_1, PullNone);
00101 InterruptIn pingEchoRiseInt(P5_1);
00102 DigitalIn p45(P4_5, PullNone);
00103 InterruptIn pingEchoFallInt(P4_5);
00104 Timer pingTimer;
00105 
00106 float cm = 0;
00107 bool pingReady = false;
00108 const float US_PER_CM = 29.387;
00109 
00110 void echoStartISR()
00111 {
00112     pingTimer.reset();
00113 }
00114 
00115 void echoStopISR()
00116 {
00117     uint32_t us = pingTimer.read_us()/2;
00118     
00119     cm = (us/ US_PER_CM);
00120     pingReady = true;
00121 }
00122 
00123 
00124 int main()
00125 {
00126     MAX32630FTHR pegasus;
00127     pegasus.init(MAX32630FTHR::VIO_3V3);
00128     
00129     static const float MAX_PULSEWIDTH_US = 50.0F;
00130     static const float MIN_PULSEWIDTH_US = -50.0F;    
00131     static const int PERIOD_US = 100;
00132     static const int MAX_PULSEWIDTH = 50;
00133     
00134     static const bool FORWARD = 0;
00135     static const bool REVERSE = 1;
00136        
00137     //Setup left motor(from driver seat)
00138     //DigitalOut leftDir(P5_6, FORWARD);    
00139     PwmOut leftDir(P5_6);
00140     leftDir.period_us(PERIOD_US);
00141     leftDir.pulsewidth_us(0);
00142     
00143     PwmOut leftPwm(P4_0);
00144     leftPwm.period_us(PERIOD_US);
00145     leftPwm.pulsewidth_us(0);
00146     
00147     //Make sure P4_2 and P4_3 are Hi-Z
00148     DigitalIn p42_hiZ(P4_2, PullNone);
00149     DigitalIn p43_hiZ(P4_3, PullNone);
00150     
00151     //Setup right motor(from driver seat)
00152     //DigitalOut rightDir(P5_4, FORWARD);
00153     PwmOut rightDir(P5_4);
00154     rightDir.period_us(PERIOD_US);
00155     rightDir.pulsewidth_us(0);
00156     
00157     PwmOut rightPwm(P5_5);
00158     rightPwm.period_us(PERIOD_US);
00159     rightPwm.pulsewidth_us(0);
00160     
00161     //Turn off RGB LEDs
00162     DigitalOut rLED(LED1, LED_OFF);
00163     DigitalOut gLED(LED2, LED_OFF);
00164     DigitalOut bLED(LED3, LED_OFF);
00165     
00166     uint32_t failures = 0;
00167     
00168     DigitalIn uSDdetect(P2_2, PullUp);
00169     static const uint32_t N = 14400;
00170     uint32_t samples = 0;
00171     float accYaxisBuff[N];
00172     float accZaxisBuff[N];
00173     float gyroXaxisBuff[N];
00174     int32_t pulseWidthBuff[N];
00175     
00176     //Setup I2C bus for IMU
00177     I2C i2cBus(P5_7, P6_0);
00178     i2cBus.frequency(400000);
00179     
00180     //Get IMU instance and configure it
00181     BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
00182     
00183     //Power up sensors in normal mode
00184     if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
00185     {
00186         printf("Failed to set gyroscope power mode\n");
00187         failures++;
00188     }
00189     
00190     wait(0.1);
00191     
00192     if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
00193     {
00194         printf("Failed to set accelerometer power mode\n");
00195         failures++;
00196     }
00197     wait(0.1);
00198     
00199     BMI160::AccConfig accConfig;
00200     BMI160::AccConfig accConfigRead;
00201     accConfig.range = BMI160::SENS_4G;
00202     accConfig.us = BMI160::ACC_US_OFF;
00203     accConfig.bwp = BMI160::ACC_BWP_2;
00204     accConfig.odr = BMI160::ACC_ODR_11;
00205     if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
00206     {
00207         if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
00208         {
00209             if((accConfig.range != accConfigRead.range) ||
00210                     (accConfig.us != accConfigRead.us) ||
00211                     (accConfig.bwp != accConfigRead.bwp) ||
00212                     (accConfig.odr != accConfigRead.odr)) 
00213             {
00214                 printf("ACC read data desn't equal set data\n\n");
00215                 printf("ACC Set Range = %d\n", accConfig.range);
00216                 printf("ACC Set UnderSampling = %d\n", accConfig.us);
00217                 printf("ACC Set BandWidthParam = %d\n", accConfig.bwp);
00218                 printf("ACC Set OutputDataRate = %d\n\n", accConfig.odr);
00219                 printf("ACC Read Range = %d\n", accConfigRead.range);
00220                 printf("ACC Read UnderSampling = %d\n", accConfigRead.us);
00221                 printf("ACC Read BandWidthParam = %d\n", accConfigRead.bwp);
00222                 printf("ACC Read OutputDataRate = %d\n\n", accConfigRead.odr);
00223                 failures++;
00224             }
00225              
00226         }
00227         else
00228         {
00229              printf("Failed to read back accelerometer configuration\n");
00230              failures++;
00231         }
00232     }
00233     else
00234     {
00235         printf("Failed to set accelerometer configuration\n");
00236         failures++;
00237     }
00238     
00239     BMI160::GyroConfig gyroConfig;
00240     BMI160::GyroConfig gyroConfigRead;
00241     gyroConfig.range = BMI160::DPS_2000;
00242     gyroConfig.bwp = BMI160::GYRO_BWP_2;
00243     gyroConfig.odr = BMI160::GYRO_ODR_11;
00244     if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
00245     {
00246         if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR)
00247         {
00248             if((gyroConfig.range != gyroConfigRead.range) ||
00249                     (gyroConfig.bwp != gyroConfigRead.bwp) ||
00250                     (gyroConfig.odr != gyroConfigRead.odr)) 
00251             {
00252                 printf("GYRO read data desn't equal set data\n\n");
00253                 printf("GYRO Set Range = %d\n", gyroConfig.range);
00254                 printf("GYRO Set BandWidthParam = %d\n", gyroConfig.bwp);
00255                 printf("GYRO Set OutputDataRate = %d\n\n", gyroConfig.odr);
00256                 printf("GYRO Read Range = %d\n", gyroConfigRead.range);
00257                 printf("GYRO Read BandWidthParam = %d\n", gyroConfigRead.bwp);
00258                 printf("GYRO Read OutputDataRate = %d\n\n", gyroConfigRead.odr);
00259                 failures++;
00260             }
00261             
00262         } 
00263         else
00264         {
00265             printf("Failed to read back gyroscope configuration\n");
00266             failures++;
00267         }
00268     }
00269     else
00270     {
00271         printf("Failed to set gyroscope configuration\n");
00272         failures++;
00273     }
00274     
00275     
00276     if(failures == 0)
00277     {
00278         printf("ACC Range = %d\n", accConfig.range);
00279         printf("ACC UnderSampling = %d\n", accConfig.us);
00280         printf("ACC BandWidthParam = %d\n", accConfig.bwp);
00281         printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
00282         printf("GYRO Range = %d\n", gyroConfig.range);
00283         printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
00284         printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
00285         wait(1.0);
00286         
00287         //Sensor data vars
00288         BMI160::SensorData accData;
00289         BMI160::SensorData gyroData;
00290         BMI160::SensorTime sensorTime;
00291         
00292         //Complementary Filter vars, filter weight K
00293         static const float K = 0.9978F;
00294         float pitch = 0.0F;
00295         
00296         //PID coefficients
00297         
00298         //tunning with Wilson, Kc = 4.2, pc = 0.166
00299 
00300         static const float DT = 0.00125F;
00301         static const float KP = 2.5F; 
00302         static const float KI = (30.0F*DT); 
00303         static const float KD = (0.05F/DT);
00304         float setPoint = 0.0F;
00305         float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT};
00306         
00307         //Control loop vars
00308         float currentError, previousError;
00309         float integral = 0.0F;
00310         float derivative = 0.0F;
00311         float pulseWidth;
00312         
00313         //Enable data ready interrupt from imu for constant loop timming
00314         imu.writeRegister(BMI160::INT_EN_1, 0x10); 
00315         //Active High PushPull output on INT1
00316         imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A); 
00317         //Map data ready interrupt to INT1
00318         imu.writeRegister(BMI160::INT_MAP_1, 0x80); 
00319         
00320         //Tie INT1 to callback fx
00321         imuIntIn.rise(&imuISR);
00322         
00323         //Tie SW2 to callback fx
00324         startStop.fall(&startStopISR);
00325         
00326         //Tie P5_2 to callback fx
00327         callibrate.fall(&callibrateISR);
00328         bool firstCal = true;
00329         
00330         //Callbacks for echo measurement
00331         //pingEchoRiseInt.fall(&echoStartISR);
00332         //pingEchoFallInt.rise(&echoStopISR);
00333         
00334         //Timer for echo measurements  
00335         //pingTimer.start();
00336         
00337         //Timer for trigger
00338         //pingTriggerTimer.attach(&triggerPing, 0.05);
00339         
00340         //Position control vars/constants
00341         //static const float PING_SP = 20.0F;
00342         //static const float PING_KP = 0.0F;
00343         //float pingCurrentError = 0.0F;
00344         
00345         //control loop timming indicator
00346         DigitalOut loopPulse(P5_3, 0);
00347         
00348         //Count for averaging setPoint offset, 2 seconds of data
00349         uint32_t offsetCount = 0;
00350         
00351         while(1)
00352         {
00353             
00354             if(cal || (firstCal == true))
00355             {
00356                 cal = false;
00357                 firstCal = false;
00358                 pitch = 0.0F;
00359                 setPoint = 0.0F;
00360                 
00361                 rLED = LED_ON;
00362                 gLED = LED_ON;
00363                 
00364                 while(offsetCount < 1600)
00365                 {
00366                     if(drdy)
00367                     {
00368                         //Clear data ready flag
00369                         drdy = false;
00370                         
00371                         //Read feedback sensors
00372                         imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
00373                         
00374                         //Feedback Block, pitch estimate in degrees
00375                         pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
00376                         
00377                         //Accumalate pitch measurements
00378                         setPoint += pitch;
00379                         offsetCount++;
00380                     }
00381                 }
00382                 
00383                 //Average measurements
00384                 setPoint = setPoint/offsetCount;
00385                 printf("setPoint = %5.2f\n\n", setPoint);
00386                 //Clear count for next time
00387                 offsetCount = 0;
00388             }
00389             
00390             
00391             while(start && (pitch > -20.0F) && (pitch < 20.0F))
00392             {                
00393                 rLED = LED_OFF;
00394                 gLED = LED_ON;
00395                 
00396                 if(drdy)
00397                 {
00398                     //Start, following takes ~456us on MAX32630FTHR with 400KHz I2C bus
00399                     //At 1600Hz ODR, ~73% of loop time is active doing something
00400                     loopPulse = !loopPulse;
00401                     
00402                     //Clear data ready flag
00403                     drdy = false;
00404                     
00405                     //Read feedback sensors
00406                     imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
00407                     
00408                     //Feedback Block, pitch estimate in degrees
00409                     pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
00410                         
00411                     //PID Block
00412                     //Error signal
00413                     currentError = (setPoint - pitch);
00414                     
00415                     //Integral term, dt is included in KI
00416                     integral = (integral + currentError);
00417                     
00418                     //Derivative term, dt is included in KD
00419                     derivative = (currentError - previousError);
00420                     
00421                     //Set right/left pulsewidth
00422                     //Just balance for now, so both left/right are the same
00423                     pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative));
00424                     
00425                     
00426                     /*
00427                     if(pingReady)
00428                     {
00429                         //Get error signal
00430                         pingReady = false;
00431                         pingCurrentError = PING_SP - cm;
00432                     }
00433                     
00434                     pulseWidth += (pingCurrentError * PING_KP);
00435                     */
00436                     
00437                     
00438                     //Clamp to maximum duty cycle and check for forward/reverse 
00439                     //based on sign of error signal (negation of pitch since setPoint = 0)
00440                     if(pulseWidth > MAX_PULSEWIDTH_US)
00441                     {
00442                         //rightDir = FORWARD;
00443                         //leftDir = FORWARD;
00444                         pulseWidth = MAX_PULSEWIDTH_US;
00445                         rightPwm.pulsewidth_us(MAX_PULSEWIDTH);
00446                         leftPwm.pulsewidth_us(MAX_PULSEWIDTH);
00447                     }
00448                     else if(pulseWidth < MIN_PULSEWIDTH_US)
00449                     {
00450                         //rightDir = REVERSE;
00451                         //leftDir = REVERSE;
00452                         pulseWidth = MIN_PULSEWIDTH_US;
00453                         rightDir.pulsewidth_us(MAX_PULSEWIDTH);
00454                         leftDir.pulsewidth_us(MAX_PULSEWIDTH);
00455                     }
00456                     else
00457                     {
00458                         if(pulseWidth < 0.0F)
00459                         {
00460                             //rightDir = REVERSE;
00461                             //leftDir = REVERSE;
00462                             rightDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
00463                             leftDir.pulsewidth_us(static_cast<uint32_t>(1 - pulseWidth));
00464                         }
00465                         else
00466                         {
00467                             //rightDir = FORWARD;
00468                             //leftDir = FORWARD;
00469                             rightPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
00470                             leftPwm.pulsewidth_us(static_cast<uint32_t>(pulseWidth));
00471                         }
00472                     }
00473                     
00474                     if(samples < N)
00475                     {
00476                         accYaxisBuff[samples] = accData.yAxis.scaled;
00477                         accZaxisBuff[samples] = accData.zAxis.scaled;
00478                         gyroXaxisBuff[samples] = gyroData.xAxis.scaled;
00479                         pulseWidthBuff[samples] = static_cast<int32_t>(pulseWidth);
00480                         samples++;
00481                     }
00482                     
00483                     //save current error for next loop
00484                     previousError = currentError;
00485                                       
00486                     //Stop
00487                     loopPulse = !loopPulse;
00488                 }
00489             }
00490             
00491             if((pitch > 20.0F) || (pitch < -20.0F))
00492             {
00493                 start = false;
00494             }
00495             
00496             pitch = 0.0F;
00497             integral = 0.0F;
00498             previousError = 0.0F;
00499             rightPwm.pulsewidth_us(0);
00500             leftPwm.pulsewidth_us(0);
00501             rightDir.pulsewidth_us(0);
00502             leftDir.pulsewidth_us(0);
00503             
00504             rLED = LED_ON;
00505             gLED = LED_ON;
00506             wait(0.25);
00507             rLED = LED_OFF;
00508             gLED = LED_OFF;
00509             wait(0.25);
00510             
00511             if(!uSDdetect && (samples > 0))
00512             {
00513                 loopCoeffs[0] = setPoint;
00514                 bLED = LED_ON;
00515                 saveData(samples, loopCoeffs, gyroXaxisBuff, accYaxisBuff, accZaxisBuff, pulseWidthBuff);
00516                 samples = 0;
00517                 bLED = LED_OFF;
00518             }   
00519         }
00520     }
00521     else
00522     {
00523         while(1)
00524         {
00525             rLED = !rLED;
00526             wait(0.25);
00527         }
00528     }
00529 }
00530 
00531 
00532 //*****************************************************************************
00533 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, 
00534               float * accY, float * accZ, int32_t * pw)
00535 {
00536     SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd");  // mosi, miso, sclk, cs
00537     FILE *fp;
00538     
00539     fp = fopen("/sd/balBot.txt", "a");
00540     if(fp != NULL)
00541     {
00542         fprintf(fp, "Samples,%d,,,,,\n", numSamples);
00543         fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]);
00544         fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]);
00545         fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]);
00546         fprintf(fp, "KI, %f,,,,,\n", loopCoeffs[3]);
00547         fprintf(fp, "KD, %f,,,,,\n", loopCoeffs[4]);
00548         fprintf(fp, "DT, %f,,,,,\n", loopCoeffs[5]);
00549         fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n");
00550         
00551         float accPitch = 0.0F;
00552         float gyroDegrees = 0.0F;
00553         float pitch = 0.0F;
00554         float K = loopCoeffs[1];
00555         float DT = loopCoeffs[5];
00556         
00557         for(uint32_t idx = 0; idx < numSamples; idx++) 
00558         {
00559             fprintf(fp, "%f,", idx*DT);
00560             fprintf(fp, "%5.4f,", accY[idx]);
00561             fprintf(fp, "%6.2f,", gyroX[idx]);
00562             gyroDegrees += (gyroX[idx] * DT);
00563             fprintf(fp, "%5.2f,", gyroDegrees);
00564             accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx]));
00565             fprintf(fp, "%5.2f,", accPitch);
00566             pitch = compFilter(K, pitch, gyroX[idx], accY[idx], accZ[idx], DT);
00567             fprintf(fp, "%5.2f,", pitch);
00568             fprintf(fp, "%d", pw[idx]);
00569             fprintf(fp, "\n");
00570         }
00571         fprintf(fp, "\n");
00572         fclose(fp);
00573     }
00574     else
00575     {
00576         printf("Failed to open file\n");
00577     }
00578 }
00579 
00580 
00581 //*****************************************************************************
00582 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT)
00583 {
00584     return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
00585 }