Charles Kim Kabiling / Mbed 2 deprecated MAX32630HSP3_IMU_HelloWorld

SelfBalancing

Dependencies:   mbed BMI160 max32630fthr USBDevice Math MAX14690

main.cpp@4:fc8ef84a7dbc, 2018-12-05 (annotated)

Committer:
CharlesMaxim
Date:
Wed Dec 05 04:44:17 2018 +0000
Revision:
4:fc8ef84a7dbc
Parent:
2:ba0a55e05168 
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EmreE 0:4b4fbf5daa1c 1 /**********************************************************************
EmreE 0:4b4fbf5daa1c 2 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
EmreE 0:4b4fbf5daa1c 3 *
EmreE 0:4b4fbf5daa1c 4 * Permission is hereby granted, free of charge, to any person obtaining a
EmreE 0:4b4fbf5daa1c 5 * copy of this software and associated documentation files (the "Software"),
EmreE 0:4b4fbf5daa1c 6 * to deal in the Software without restriction, including without limitation
EmreE 0:4b4fbf5daa1c 7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
EmreE 0:4b4fbf5daa1c 8 * and/or sell copies of the Software, and to permit persons to whom the
EmreE 0:4b4fbf5daa1c 9 * Software is furnished to do so, subject to the following conditions:
EmreE 0:4b4fbf5daa1c 10 *
EmreE 0:4b4fbf5daa1c 11 * The above copyright notice and this permission notice shall be included
EmreE 0:4b4fbf5daa1c 12 * in all copies or substantial portions of the Software.
EmreE 0:4b4fbf5daa1c 13 *
EmreE 0:4b4fbf5daa1c 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
EmreE 0:4b4fbf5daa1c 15 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
EmreE 0:4b4fbf5daa1c 16 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
EmreE 0:4b4fbf5daa1c 17 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
EmreE 0:4b4fbf5daa1c 18 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
EmreE 0:4b4fbf5daa1c 19 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
EmreE 0:4b4fbf5daa1c 20 * OTHER DEALINGS IN THE SOFTWARE.
EmreE 0:4b4fbf5daa1c 21 *
EmreE 0:4b4fbf5daa1c 22 * Except as contained in this notice, the name of Maxim Integrated
EmreE 0:4b4fbf5daa1c 23 * Products, Inc. shall not be used except as stated in the Maxim Integrated
EmreE 0:4b4fbf5daa1c 24 * Products, Inc. Branding Policy.
EmreE 0:4b4fbf5daa1c 25 *
EmreE 0:4b4fbf5daa1c 26 * The mere transfer of this software does not imply any licenses
EmreE 0:4b4fbf5daa1c 27 * of trade secrets, proprietary technology, copyrights, patents,
EmreE 0:4b4fbf5daa1c 28 * trademarks, maskwork rights, or any other form of intellectual
EmreE 0:4b4fbf5daa1c 29 * property whatsoever. Maxim Integrated Products, Inc. retains all
EmreE 0:4b4fbf5daa1c 30 * ownership rights.
EmreE 0:4b4fbf5daa1c 31 **********************************************************************/
EmreE 0:4b4fbf5daa1c 32
EmreE 0:4b4fbf5daa1c 33
EmreE 0:4b4fbf5daa1c 34 #include "mbed.h"
EmreE 0:4b4fbf5daa1c 35 #include "bmi160.h"
CharlesMaxim 4:fc8ef84a7dbc 36 //#include "max32630hsp.h"
CharlesMaxim 4:fc8ef84a7dbc 37 #include "max32630fthr.h"
CharlesMaxim 4:fc8ef84a7dbc 38 #include "USBSerial.h"
CharlesMaxim 4:fc8ef84a7dbc 39 #include "stdlib.h"
EmreE 0:4b4fbf5daa1c 40
CharlesMaxim 4:fc8ef84a7dbc 41 //MAX32630HSP icarus(MAX32630HSP::VIO_3V3);
CharlesMaxim 4:fc8ef84a7dbc 42 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
CharlesMaxim 4:fc8ef84a7dbc 43
CharlesMaxim 4:fc8ef84a7dbc 44 Serial daplink(P2_1, P2_0);
EmreE 0:4b4fbf5daa1c 45
CharlesMaxim 4:fc8ef84a7dbc 46 DigitalOut rLED(P2_4, LED_ON);
CharlesMaxim 4:fc8ef84a7dbc 47 DigitalOut gLED(P2_5, LED_ON);
CharlesMaxim 4:fc8ef84a7dbc 48 DigitalOut bLED(P2_6, LED_ON);
EmreE 0:4b4fbf5daa1c 49
CharlesMaxim 4:fc8ef84a7dbc 50 DigitalOut M_1(P3_5);
CharlesMaxim 4:fc8ef84a7dbc 51 DigitalOut M_2(P3_4);
CharlesMaxim 4:fc8ef84a7dbc 52
CharlesMaxim 4:fc8ef84a7dbc 53
CharlesMaxim 4:fc8ef84a7dbc 54 USBSerial pc(USBTX,USBRX);
EmreE 0:4b4fbf5daa1c 55
EmreE 0:4b4fbf5daa1c 56 I2C i2cBus(I2C2_SDA, I2C2_SCL);
EmreE 0:4b4fbf5daa1c 57
CharlesMaxim 4:fc8ef84a7dbc 58
CharlesMaxim 4:fc8ef84a7dbc 59
EmreE 0:4b4fbf5daa1c 60 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
EmreE 0:4b4fbf5daa1c 61
CharlesMaxim 4:fc8ef84a7dbc 62
EmreE 0:4b4fbf5daa1c 63 void dumpImuRegisters(BMI160 &imu);
EmreE 0:4b4fbf5daa1c 64 void printRegister(BMI160 &imu, BMI160::Registers reg);
EmreE 0:4b4fbf5daa1c 65 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
CharlesMaxim 4:fc8ef84a7dbc 66 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data);
EmreE 0:4b4fbf5daa1c 67
EmreE 0:4b4fbf5daa1c 68 int main()
EmreE 0:4b4fbf5daa1c 69 {
EmreE 0:4b4fbf5daa1c 70 i2cBus.frequency(400000);
CharlesMaxim 4:fc8ef84a7dbc 71 writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500);
CharlesMaxim 4:fc8ef84a7dbc 72 writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13);
CharlesMaxim 4:fc8ef84a7dbc 73 writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE );
CharlesMaxim 4:fc8ef84a7dbc 74 wait(0.5);
CharlesMaxim 4:fc8ef84a7dbc 75 writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC);
CharlesMaxim 4:fc8ef84a7dbc 76 gLED = 1;
CharlesMaxim 4:fc8ef84a7dbc 77 bLED = 0;
CharlesMaxim 4:fc8ef84a7dbc 78 wait(1);
CharlesMaxim 4:fc8ef84a7dbc 79
CharlesMaxim 4:fc8ef84a7dbc 80 writeReg(imu, BMI160::CMD, BMI160::FOC_START);
CharlesMaxim 4:fc8ef84a7dbc 81
CharlesMaxim 4:fc8ef84a7dbc 82 wait(1);
CharlesMaxim 4:fc8ef84a7dbc 83 gLED = 0;
CharlesMaxim 4:fc8ef84a7dbc 84 bLED = 0;
EmreE 0:4b4fbf5daa1c 85
EmreE 0:4b4fbf5daa1c 86 pc.printf("\033[H"); //home
EmreE 0:4b4fbf5daa1c 87 pc.printf("\033[0J"); //erase from cursor to end of screen
EmreE 0:4b4fbf5daa1c 88
EmreE 0:4b4fbf5daa1c 89 uint32_t failures = 0;
EmreE 0:4b4fbf5daa1c 90
EmreE 0:4b4fbf5daa1c 91 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 92 {
EmreE 0:4b4fbf5daa1c 93 pc.printf("Failed to set gyroscope power mode\r\n");
EmreE 0:4b4fbf5daa1c 94 failures++;
EmreE 0:4b4fbf5daa1c 95 }
EmreE 0:4b4fbf5daa1c 96 wait_ms(100);
EmreE 0:4b4fbf5daa1c 97
EmreE 0:4b4fbf5daa1c 98 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 99 {
EmreE 0:4b4fbf5daa1c 100 pc.printf("Failed to set accelerometer power mode\r\n");
EmreE 0:4b4fbf5daa1c 101 failures++;
EmreE 0:4b4fbf5daa1c 102 }
EmreE 0:4b4fbf5daa1c 103 wait_ms(100);
EmreE 0:4b4fbf5daa1c 104
EmreE 0:4b4fbf5daa1c 105
EmreE 0:4b4fbf5daa1c 106 BMI160::AccConfig accConfig;
EmreE 0:4b4fbf5daa1c 107 //example of using getSensorConfig
EmreE 0:4b4fbf5daa1c 108 if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 109 {
EmreE 0:4b4fbf5daa1c 110 pc.printf("ACC Range = %d\r\n", accConfig.range);
EmreE 0:4b4fbf5daa1c 111 pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
EmreE 0:4b4fbf5daa1c 112 pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
EmreE 0:4b4fbf5daa1c 113 pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
EmreE 0:4b4fbf5daa1c 114 }
EmreE 0:4b4fbf5daa1c 115 else
EmreE 0:4b4fbf5daa1c 116 {
EmreE 0:4b4fbf5daa1c 117 pc.printf("Failed to get accelerometer configuration\r\n");
EmreE 0:4b4fbf5daa1c 118 failures++;
EmreE 0:4b4fbf5daa1c 119 }
EmreE 0:4b4fbf5daa1c 120
EmreE 0:4b4fbf5daa1c 121 //example of setting user defined configuration
CharlesMaxim 4:fc8ef84a7dbc 122 accConfig.range = BMI160::SENS_2G; //rage is 2g
CharlesMaxim 4:fc8ef84a7dbc 123 accConfig.us = BMI160::ACC_US_OFF; //undersampling is off
CharlesMaxim 4:fc8ef84a7dbc 124 accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles
CharlesMaxim 4:fc8ef84a7dbc 125 accConfig.odr = BMI160::ACC_ODR_9; //output data rate
EmreE 0:4b4fbf5daa1c 126 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 127 {
EmreE 0:4b4fbf5daa1c 128 pc.printf("ACC Range = %d\r\n", accConfig.range);
EmreE 0:4b4fbf5daa1c 129 pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
EmreE 0:4b4fbf5daa1c 130 pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
EmreE 0:4b4fbf5daa1c 131 pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
EmreE 0:4b4fbf5daa1c 132 }
EmreE 0:4b4fbf5daa1c 133 else
EmreE 0:4b4fbf5daa1c 134 {
EmreE 0:4b4fbf5daa1c 135 pc.printf("Failed to set accelerometer configuration\r\n");
EmreE 0:4b4fbf5daa1c 136 failures++;
EmreE 0:4b4fbf5daa1c 137 }
EmreE 0:4b4fbf5daa1c 138
EmreE 0:4b4fbf5daa1c 139 BMI160::GyroConfig gyroConfig;
EmreE 0:4b4fbf5daa1c 140 if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 141 {
EmreE 0:4b4fbf5daa1c 142 pc.printf("GYRO Range = %d\r\n", gyroConfig.range);
EmreE 0:4b4fbf5daa1c 143 pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
EmreE 0:4b4fbf5daa1c 144 pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
EmreE 0:4b4fbf5daa1c 145 }
EmreE 0:4b4fbf5daa1c 146 else
EmreE 0:4b4fbf5daa1c 147 {
EmreE 0:4b4fbf5daa1c 148 pc.printf("Failed to get gyroscope configuration\r\n");
EmreE 0:4b4fbf5daa1c 149 failures++;
EmreE 0:4b4fbf5daa1c 150 }
EmreE 0:4b4fbf5daa1c 151
EmreE 0:4b4fbf5daa1c 152 wait(1.0);
EmreE 0:4b4fbf5daa1c 153 pc.printf("\033[H"); //home
EmreE 0:4b4fbf5daa1c 154 pc.printf("\033[0J"); //erase from cursor to end of screen
EmreE 0:4b4fbf5daa1c 155
EmreE 0:4b4fbf5daa1c 156 if(failures == 0)
EmreE 0:4b4fbf5daa1c 157 {
CharlesMaxim 4:fc8ef84a7dbc 158
CharlesMaxim 4:fc8ef84a7dbc 159
EmreE 0:4b4fbf5daa1c 160 float imuTemperature;
CharlesMaxim 4:fc8ef84a7dbc 161
CharlesMaxim 4:fc8ef84a7dbc 162 double xDeviation, yDeviation, zDeviation;
CharlesMaxim 4:fc8ef84a7dbc 163 double prevGyroX, prevGyroY, prevGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 164 double currentGyroX, currentGyroY, currentGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 165 double diffGyroX, diffGyroY, diffGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 166
CharlesMaxim 4:fc8ef84a7dbc 167 double xDisplacement, yDisplacement, zDisplacement;
CharlesMaxim 4:fc8ef84a7dbc 168 double currentAccX, currentAccY, currentAccZ;
CharlesMaxim 4:fc8ef84a7dbc 169 double prevAccX, prevAccY, prevAccZ;
CharlesMaxim 4:fc8ef84a7dbc 170 double diffAccX, diffAccY, diffAccZ;
CharlesMaxim 4:fc8ef84a7dbc 171
CharlesMaxim 4:fc8ef84a7dbc 172 double xVelocity, yVelocity, zVelocity;
CharlesMaxim 4:fc8ef84a7dbc 173 double timeDiff, time_1, time_2;
CharlesMaxim 4:fc8ef84a7dbc 174 bool timeFlag = false;
EmreE 0:4b4fbf5daa1c 175 BMI160::SensorData accData;
EmreE 0:4b4fbf5daa1c 176 BMI160::SensorData gyroData;
EmreE 0:4b4fbf5daa1c 177 BMI160::SensorTime sensorTime;
CharlesMaxim 4:fc8ef84a7dbc 178
CharlesMaxim 4:fc8ef84a7dbc 179 //PwmPin = 1;
EmreE 0:4b4fbf5daa1c 180
EmreE 0:4b4fbf5daa1c 181 while(1)
EmreE 0:4b4fbf5daa1c 182 {
CharlesMaxim 4:fc8ef84a7dbc 183
EmreE 0:4b4fbf5daa1c 184 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
EmreE 0:4b4fbf5daa1c 185 imu.getTemperature(&imuTemperature);
CharlesMaxim 4:fc8ef84a7dbc 186
CharlesMaxim 4:fc8ef84a7dbc 187 //printRegister(imu, BMI160::GYR_CONF);
CharlesMaxim 4:fc8ef84a7dbc 188
CharlesMaxim 4:fc8ef84a7dbc 189 if(timeFlag == true){
CharlesMaxim 4:fc8ef84a7dbc 190
CharlesMaxim 4:fc8ef84a7dbc 191 currentGyroX = gyroData.xAxis.scaled;
CharlesMaxim 4:fc8ef84a7dbc 192 currentAccX = accData.xAxis.scaled;
EmreE 0:4b4fbf5daa1c 193
CharlesMaxim 4:fc8ef84a7dbc 194
CharlesMaxim 4:fc8ef84a7dbc 195 diffGyroX = abs(currentGyroX - prevGyroX);
CharlesMaxim 4:fc8ef84a7dbc 196 diffAccX = abs(currentAccX - prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 197
CharlesMaxim 4:fc8ef84a7dbc 198 time_2 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 199 timeDiff = time_2 - time_1;
CharlesMaxim 4:fc8ef84a7dbc 200
CharlesMaxim 4:fc8ef84a7dbc 201 if (diffGyroX > 2){
CharlesMaxim 4:fc8ef84a7dbc 202 xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff));
CharlesMaxim 4:fc8ef84a7dbc 203 }
CharlesMaxim 4:fc8ef84a7dbc 204 if (diffAccX > 0.009){
CharlesMaxim 4:fc8ef84a7dbc 205 xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 206 xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 207 }
CharlesMaxim 4:fc8ef84a7dbc 208
CharlesMaxim 4:fc8ef84a7dbc 209 // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 210
CharlesMaxim 4:fc8ef84a7dbc 211 //control motor for proportional linearity
CharlesMaxim 4:fc8ef84a7dbc 212
CharlesMaxim 4:fc8ef84a7dbc 213 if(xDeviation < 0.0)
CharlesMaxim 4:fc8ef84a7dbc 214 {
CharlesMaxim 4:fc8ef84a7dbc 215 M_1 = 0;
CharlesMaxim 4:fc8ef84a7dbc 216 M_2 = 1;
CharlesMaxim 4:fc8ef84a7dbc 217 pc.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 218 }
CharlesMaxim 4:fc8ef84a7dbc 219 else{
CharlesMaxim 4:fc8ef84a7dbc 220 M_1 = 1;
CharlesMaxim 4:fc8ef84a7dbc 221 M_2 = 0;
CharlesMaxim 4:fc8ef84a7dbc 222 pc.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 223 }
CharlesMaxim 4:fc8ef84a7dbc 224
CharlesMaxim 4:fc8ef84a7dbc 225 //------------------------
CharlesMaxim 4:fc8ef84a7dbc 226
CharlesMaxim 4:fc8ef84a7dbc 227
CharlesMaxim 4:fc8ef84a7dbc 228 //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 229 // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 230 //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement);
EmreE 0:4b4fbf5daa1c 231
CharlesMaxim 4:fc8ef84a7dbc 232 prevGyroX = currentGyroX;
CharlesMaxim 4:fc8ef84a7dbc 233 prevAccX = currentAccX;
CharlesMaxim 4:fc8ef84a7dbc 234 time_1 = time_2;
CharlesMaxim 4:fc8ef84a7dbc 235 }
CharlesMaxim 4:fc8ef84a7dbc 236 else{
CharlesMaxim 4:fc8ef84a7dbc 237 time_1 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 238 timeFlag = true;
CharlesMaxim 4:fc8ef84a7dbc 239 }
CharlesMaxim 4:fc8ef84a7dbc 240
CharlesMaxim 4:fc8ef84a7dbc 241
EmreE 0:4b4fbf5daa1c 242 gLED = !gLED;
CharlesMaxim 4:fc8ef84a7dbc 243 wait_ms(1);
EmreE 0:4b4fbf5daa1c 244 }
EmreE 0:4b4fbf5daa1c 245 }
EmreE 0:4b4fbf5daa1c 246 else
EmreE 0:4b4fbf5daa1c 247 {
EmreE 0:4b4fbf5daa1c 248 while(1)
EmreE 0:4b4fbf5daa1c 249 {
EmreE 0:4b4fbf5daa1c 250 rLED = !rLED;
CharlesMaxim 4:fc8ef84a7dbc 251 wait(0.6);
EmreE 0:4b4fbf5daa1c 252 }
EmreE 0:4b4fbf5daa1c 253 }
EmreE 0:4b4fbf5daa1c 254 }
EmreE 0:4b4fbf5daa1c 255
EmreE 0:4b4fbf5daa1c 256
EmreE 0:4b4fbf5daa1c 257 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 258 void dumpImuRegisters(BMI160 &imu)
EmreE 0:4b4fbf5daa1c 259 {
EmreE 0:4b4fbf5daa1c 260 printRegister(imu, BMI160::CHIP_ID);
EmreE 0:4b4fbf5daa1c 261 printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
EmreE 0:4b4fbf5daa1c 262 printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
EmreE 0:4b4fbf5daa1c 263 printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
EmreE 0:4b4fbf5daa1c 264 printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
EmreE 0:4b4fbf5daa1c 265 printRegister(imu, BMI160::CMD);
EmreE 0:4b4fbf5daa1c 266 pc.printf("\r\n");
EmreE 0:4b4fbf5daa1c 267 }
EmreE 0:4b4fbf5daa1c 268
EmreE 0:4b4fbf5daa1c 269
EmreE 0:4b4fbf5daa1c 270 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 271 void printRegister(BMI160 &imu, BMI160::Registers reg)
EmreE 0:4b4fbf5daa1c 272 {
EmreE 0:4b4fbf5daa1c 273 uint8_t data;
EmreE 0:4b4fbf5daa1c 274 if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 275 {
EmreE 0:4b4fbf5daa1c 276 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
CharlesMaxim 4:fc8ef84a7dbc 277 daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
EmreE 0:4b4fbf5daa1c 278 }
EmreE 0:4b4fbf5daa1c 279 else
EmreE 0:4b4fbf5daa1c 280 {
EmreE 0:4b4fbf5daa1c 281 pc.printf("Failed to read register\r\n");
EmreE 0:4b4fbf5daa1c 282 }
EmreE 0:4b4fbf5daa1c 283 }
EmreE 0:4b4fbf5daa1c 284
CharlesMaxim 4:fc8ef84a7dbc 285 //*****************************************************************************
CharlesMaxim 4:fc8ef84a7dbc 286 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
CharlesMaxim 4:fc8ef84a7dbc 287 {
CharlesMaxim 4:fc8ef84a7dbc 288 imu.writeRegister(reg, data);
CharlesMaxim 4:fc8ef84a7dbc 289
CharlesMaxim 4:fc8ef84a7dbc 290
CharlesMaxim 4:fc8ef84a7dbc 291 }
CharlesMaxim 4:fc8ef84a7dbc 292
EmreE 0:4b4fbf5daa1c 293
EmreE 0:4b4fbf5daa1c 294 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 295 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
EmreE 0:4b4fbf5daa1c 296 {
EmreE 0:4b4fbf5daa1c 297 uint8_t numBytes = ((stopReg - startReg) + 1);
Emre.Eken 2:ba0a55e05168 298 uint8_t buff[32];
EmreE 0:4b4fbf5daa1c 299 uint8_t offset = static_cast<uint8_t>(startReg);
EmreE 0:4b4fbf5daa1c 300
EmreE 0:4b4fbf5daa1c 301 if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 302 {
EmreE 0:4b4fbf5daa1c 303 for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
EmreE 0:4b4fbf5daa1c 304 {
EmreE 0:4b4fbf5daa1c 305 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
EmreE 0:4b4fbf5daa1c 306 }
EmreE 0:4b4fbf5daa1c 307 }
EmreE 0:4b4fbf5daa1c 308 else
EmreE 0:4b4fbf5daa1c 309 {
EmreE 0:4b4fbf5daa1c 310 pc.printf("Failed to read block\r\n");
EmreE 0:4b4fbf5daa1c 311 }
EmreE 0:4b4fbf5daa1c 312 }
CharlesMaxim 4:fc8ef84a7dbc 313
CharlesMaxim 4:fc8ef84a7dbc 314
CharlesMaxim 4:fc8ef84a7dbc 315 /* An example for configuring FOC for accel and gyro data */