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 #include "mbed.h"
00035 #include "bmi160.h"
00036 //#include "max32630hsp.h"
00037 #include "max32630fthr.h"
00038 #include "USBSerial.h"
00039 #include "stdlib.h"
00040 #include "math.h"
00041 
00042 //MAX32630HSP icarus(MAX32630HSP::VIO_3V3);
00043 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
00044 
00045 Serial daplink(P2_1, P2_0);
00046 
00047 DigitalOut rLED(P2_4, LED_ON);
00048 DigitalOut gLED(P2_5, LED_ON);
00049 DigitalOut bLED(P2_6, LED_ON);
00050 
00051 PwmOut PWM(P3_5);
00052 DigitalOut Dir(P3_4);
00053 
00054 
00055 USBSerial pc(USBTX,USBRX);
00056 
00057 I2C i2cBus(I2C2_SDA, I2C2_SCL);
00058 
00059 
00060 
00061 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
00062 
00063 
00064 void dumpImuRegisters(BMI160 &imu);
00065 void printRegister(BMI160 &imu, BMI160::Registers reg);
00066 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
00067 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data);
00068 //float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT);
00069 
00070 
00071 float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT)
00072 {
00073     return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
00074 }
00075 
00076 
00077 int main()
00078 {
00079     i2cBus.frequency(400000);
00080     writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500);
00081     writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13);
00082     writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE );
00083     wait(0.5);
00084     writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC);
00085     gLED = 1;
00086     bLED = 0;
00087     wait(1);
00088     
00089     writeReg(imu, BMI160::CMD, BMI160::FOC_START);
00090     
00091     wait(1);
00092     gLED = 0;
00093     bLED = 0;
00094 
00095     pc.printf("\033[H");  //home
00096     pc.printf("\033[0J");  //erase from cursor to end of screen
00097 
00098     uint32_t failures = 0;
00099 
00100     if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
00101     {
00102         pc.printf("Failed to set gyroscope power mode\r\n");
00103         failures++;
00104     }
00105     wait_ms(100);
00106 
00107     if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
00108     {
00109         pc.printf("Failed to set accelerometer power mode\r\n");
00110         failures++;
00111     }
00112     wait_ms(100);
00113 
00114 
00115     BMI160::AccConfig accConfig;
00116     //example of using getSensorConfig
00117     if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
00118     {
00119         pc.printf("ACC Range = %d\r\n", accConfig.range);
00120         pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
00121         pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
00122         pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
00123     }
00124     else
00125     {
00126         pc.printf("Failed to get accelerometer configuration\r\n");
00127         failures++;
00128     }
00129 
00130     //example of setting user defined configuration
00131     accConfig.range = BMI160::SENS_2G;  //rage is 2g
00132     accConfig.us = BMI160::ACC_US_OFF;  //undersampling is off 
00133     accConfig.bwp = BMI160::ACC_BWP_0;  //average 4 cycles
00134     accConfig.odr = BMI160::ACC_ODR_9;  //output data rate
00135     if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
00136     {
00137         pc.printf("ACC Range = %d\r\n", accConfig.range);
00138         pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
00139         pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
00140         pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
00141     }
00142     else
00143     {
00144         pc.printf("Failed to set accelerometer configuration\r\n");
00145         failures++;
00146     }
00147 
00148     BMI160::GyroConfig gyroConfig;
00149     if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
00150     {
00151         pc.printf("GYRO Range = %d\r\n", gyroConfig.range);
00152         pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
00153         pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
00154         
00155         
00156     }
00157     else
00158     {
00159         pc.printf("Failed to get gyroscope configuration\r\n");
00160         failures++;
00161     }
00162 
00163     wait(1.0);
00164     pc.printf("\033[H");  //home
00165     pc.printf("\033[0J");  //erase from cursor to end of screen
00166 
00167     if(failures == 0)
00168     {
00169         
00170         
00171         float imuTemperature;
00172         float dutyCycle;
00173         
00174         double xDeviation, yDeviation, zDeviation;
00175         double prevGyroX, prevGyroY, prevGyroZ;
00176         double currentGyroX, currentGyroY, currentGyroZ;
00177         double diffGyroX, diffGyroY, diffGyroZ;
00178         
00179         double xDisplacement, yDisplacement, zDisplacement;
00180         double currentAccX, currentAccY, currentAccZ;
00181         double prevAccX, prevAccY, prevAccZ;
00182         double diffAccX, diffAccY, diffAccZ;
00183         
00184         double xVelocity, yVelocity, zVelocity;
00185         double timeDiff, time_1, time_2;
00186         bool timeFlag = false;
00187         BMI160::SensorData accData;
00188         BMI160::SensorData gyroData;
00189         BMI160::SensorTime sensorTime;
00190         
00191         //PwmPin = 1;
00192         float apitch = 0;
00193         float k = 0.6;
00194         
00195         time_1 = sensorTime.seconds;
00196         while(1)
00197         {
00198             
00199             imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
00200             imu.getTemperature(&imuTemperature);
00201             
00202             time_2 = sensorTime.seconds;
00203             apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
00204             pc.printf("Drift: %s%4.3f\r\n", "\033[K", apitch);
00205           
00206             
00207             //dutyCycle = (apitch * (0.2 / 10)) + 0.8;
00208           //  dutyCycle = fabsf(dutyCycle);
00209           //  pc.printf("PWM: %s%4.3f\r\n", "\033[K", dutyCycle);
00210             
00211               time_1 = time_2;
00212         /*    
00213             if(dutyCycle < 0 ){
00214                 dutyCycle = 0 - dutyCycle;
00215             }
00216          */   
00217             if(apitch <= 0){
00218                 PWM = 0.5;
00219                 Dir = 1;    
00220             }
00221             
00222             if(apitch > 0){
00223                 PWM = 0.5;
00224                 Dir = 0;    
00225             }
00226             
00227              //printRegister(imu, BMI160::GYR_CONF);
00228              
00229        }
00230     }
00231 }     
00232              
00233 /*             
00234             if(timeFlag == true){
00235                 
00236                 currentGyroX = gyroData.xAxis.scaled;
00237                 currentAccX = accData.xAxis.scaled;
00238 
00239                 
00240                 diffGyroX = abs(currentGyroX - prevGyroX);
00241                 diffAccX = abs(currentAccX - prevAccX);
00242                 
00243                 time_2 = sensorTime.seconds;
00244                 timeDiff = time_2 - time_1;
00245     
00246                 if (diffGyroX > 2){
00247                     xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff)); 
00248                 }  
00249                 if (diffAccX > 0.009){
00250                     xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff);
00251                     xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff);
00252                 }
00253         
00254                // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation);
00255                 
00256                 //control motor for proportional linearity
00257                 
00258                     if(xDeviation < 0.0)
00259                 {
00260                     M_1 = 0;
00261                     M_2 = 1;
00262                     daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
00263                 }
00264                     else{
00265                     M_1 = 1;
00266                     M_2 = 0;
00267                     daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
00268                 }
00269                 
00270                 //------------------------
00271                 
00272                 
00273                 //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX);
00274                // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff);
00275                 //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement);
00276 
00277                 prevGyroX = currentGyroX;
00278                 prevAccX = currentAccX;
00279                 time_1 = time_2;
00280             }
00281             else{
00282                 time_1 = sensorTime.seconds;
00283                 timeFlag = true;
00284             }
00285         
00286             
00287             gLED = !gLED;
00288             wait_ms(1);
00289         }
00290     }
00291     else
00292     {
00293         while(1)
00294         {
00295             rLED = !rLED;
00296             wait(0.6);
00297         }
00298     }
00299 */
00300 
00301 
00302 
00303 
00304 //*****************************************************************************
00305 void dumpImuRegisters(BMI160 &imu)
00306 {
00307     printRegister(imu, BMI160::CHIP_ID);
00308     printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
00309     printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
00310     printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
00311     printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
00312     printRegister(imu, BMI160::CMD);
00313     pc.printf("\r\n");
00314 }
00315 
00316 
00317 //*****************************************************************************
00318 void printRegister(BMI160 &imu, BMI160::Registers reg)
00319 {
00320     uint8_t data;
00321     if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
00322     {
00323         pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
00324          daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
00325     }
00326     else
00327     {
00328         pc.printf("Failed to read register\r\n");
00329     }
00330 }
00331 
00332 //*****************************************************************************
00333 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
00334 {
00335     imu.writeRegister(reg, data);
00336     
00337     
00338 }
00339 
00340 
00341 //*****************************************************************************
00342 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
00343 {
00344     uint8_t numBytes = ((stopReg - startReg) + 1);
00345     uint8_t buff[32];
00346     uint8_t offset = static_cast<uint8_t>(startReg);
00347 
00348     if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
00349     {
00350         for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
00351         {
00352             pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
00353         }
00354     }
00355     else
00356     {
00357         pc.printf("Failed to read block\r\n");
00358     }
00359 }
00360 
00361 
00362 /* An example for configuring FOC for accel and gyro data */