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