TSDA Robotics / Mbed 2 deprecated Maxim_Squeeks

Final Tree

Dependencies:   mbed BMI160 max32630fthr_pitch USBDevice Math

main.cpp@5:f6727800e43f, 2018-12-13 (annotated)

Committer:
bjbance
Date:
Thu Dec 13 11:17:03 2018 +0000
Revision:
5:f6727800e43f
Parent:
4:fc8ef84a7dbc
Child:
7:b33be863fbb5
pitch correction;

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"
bjbance 5:f6727800e43f 40 #include "math.h"
EmreE 0:4b4fbf5daa1c 41
CharlesMaxim 4:fc8ef84a7dbc 42 //MAX32630HSP icarus(MAX32630HSP::VIO_3V3);
CharlesMaxim 4:fc8ef84a7dbc 43 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
CharlesMaxim 4:fc8ef84a7dbc 44
CharlesMaxim 4:fc8ef84a7dbc 45 Serial daplink(P2_1, P2_0);
EmreE 0:4b4fbf5daa1c 46
CharlesMaxim 4:fc8ef84a7dbc 47 DigitalOut rLED(P2_4, LED_ON);
CharlesMaxim 4:fc8ef84a7dbc 48 DigitalOut gLED(P2_5, LED_ON);
CharlesMaxim 4:fc8ef84a7dbc 49 DigitalOut bLED(P2_6, LED_ON);
EmreE 0:4b4fbf5daa1c 50
CharlesMaxim 4:fc8ef84a7dbc 51 DigitalOut M_1(P3_5);
CharlesMaxim 4:fc8ef84a7dbc 52 DigitalOut M_2(P3_4);
CharlesMaxim 4:fc8ef84a7dbc 53
CharlesMaxim 4:fc8ef84a7dbc 54
CharlesMaxim 4:fc8ef84a7dbc 55 USBSerial pc(USBTX,USBRX);
EmreE 0:4b4fbf5daa1c 56
EmreE 0:4b4fbf5daa1c 57 I2C i2cBus(I2C2_SDA, I2C2_SCL);
EmreE 0:4b4fbf5daa1c 58
CharlesMaxim 4:fc8ef84a7dbc 59
CharlesMaxim 4:fc8ef84a7dbc 60
EmreE 0:4b4fbf5daa1c 61 BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
EmreE 0:4b4fbf5daa1c 62
CharlesMaxim 4:fc8ef84a7dbc 63
EmreE 0:4b4fbf5daa1c 64 void dumpImuRegisters(BMI160 &imu);
EmreE 0:4b4fbf5daa1c 65 void printRegister(BMI160 &imu, BMI160::Registers reg);
EmreE 0:4b4fbf5daa1c 66 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
CharlesMaxim 4:fc8ef84a7dbc 67 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data);
bjbance 5:f6727800e43f 68 //float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT);
bjbance 5:f6727800e43f 69
bjbance 5:f6727800e43f 70
bjbance 5:f6727800e43f 71 float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT)
bjbance 5:f6727800e43f 72 {
bjbance 5:f6727800e43f 73 return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
bjbance 5:f6727800e43f 74 }
bjbance 5:f6727800e43f 75
EmreE 0:4b4fbf5daa1c 76
EmreE 0:4b4fbf5daa1c 77 int main()
EmreE 0:4b4fbf5daa1c 78 {
EmreE 0:4b4fbf5daa1c 79 i2cBus.frequency(400000);
CharlesMaxim 4:fc8ef84a7dbc 80 writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500);
CharlesMaxim 4:fc8ef84a7dbc 81 writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13);
CharlesMaxim 4:fc8ef84a7dbc 82 writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE );
CharlesMaxim 4:fc8ef84a7dbc 83 wait(0.5);
CharlesMaxim 4:fc8ef84a7dbc 84 writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC);
CharlesMaxim 4:fc8ef84a7dbc 85 gLED = 1;
CharlesMaxim 4:fc8ef84a7dbc 86 bLED = 0;
CharlesMaxim 4:fc8ef84a7dbc 87 wait(1);
CharlesMaxim 4:fc8ef84a7dbc 88
CharlesMaxim 4:fc8ef84a7dbc 89 writeReg(imu, BMI160::CMD, BMI160::FOC_START);
CharlesMaxim 4:fc8ef84a7dbc 90
CharlesMaxim 4:fc8ef84a7dbc 91 wait(1);
CharlesMaxim 4:fc8ef84a7dbc 92 gLED = 0;
CharlesMaxim 4:fc8ef84a7dbc 93 bLED = 0;
EmreE 0:4b4fbf5daa1c 94
EmreE 0:4b4fbf5daa1c 95 pc.printf("\033[H"); //home
EmreE 0:4b4fbf5daa1c 96 pc.printf("\033[0J"); //erase from cursor to end of screen
EmreE 0:4b4fbf5daa1c 97
EmreE 0:4b4fbf5daa1c 98 uint32_t failures = 0;
EmreE 0:4b4fbf5daa1c 99
EmreE 0:4b4fbf5daa1c 100 if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 101 {
EmreE 0:4b4fbf5daa1c 102 pc.printf("Failed to set gyroscope power mode\r\n");
EmreE 0:4b4fbf5daa1c 103 failures++;
EmreE 0:4b4fbf5daa1c 104 }
EmreE 0:4b4fbf5daa1c 105 wait_ms(100);
EmreE 0:4b4fbf5daa1c 106
EmreE 0:4b4fbf5daa1c 107 if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 108 {
EmreE 0:4b4fbf5daa1c 109 pc.printf("Failed to set accelerometer power mode\r\n");
EmreE 0:4b4fbf5daa1c 110 failures++;
EmreE 0:4b4fbf5daa1c 111 }
EmreE 0:4b4fbf5daa1c 112 wait_ms(100);
EmreE 0:4b4fbf5daa1c 113
EmreE 0:4b4fbf5daa1c 114
EmreE 0:4b4fbf5daa1c 115 BMI160::AccConfig accConfig;
EmreE 0:4b4fbf5daa1c 116 //example of using getSensorConfig
EmreE 0:4b4fbf5daa1c 117 if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 118 {
EmreE 0:4b4fbf5daa1c 119 pc.printf("ACC Range = %d\r\n", accConfig.range);
EmreE 0:4b4fbf5daa1c 120 pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
EmreE 0:4b4fbf5daa1c 121 pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
EmreE 0:4b4fbf5daa1c 122 pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
EmreE 0:4b4fbf5daa1c 123 }
EmreE 0:4b4fbf5daa1c 124 else
EmreE 0:4b4fbf5daa1c 125 {
EmreE 0:4b4fbf5daa1c 126 pc.printf("Failed to get accelerometer configuration\r\n");
EmreE 0:4b4fbf5daa1c 127 failures++;
EmreE 0:4b4fbf5daa1c 128 }
EmreE 0:4b4fbf5daa1c 129
EmreE 0:4b4fbf5daa1c 130 //example of setting user defined configuration
CharlesMaxim 4:fc8ef84a7dbc 131 accConfig.range = BMI160::SENS_2G; //rage is 2g
CharlesMaxim 4:fc8ef84a7dbc 132 accConfig.us = BMI160::ACC_US_OFF; //undersampling is off
CharlesMaxim 4:fc8ef84a7dbc 133 accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles
CharlesMaxim 4:fc8ef84a7dbc 134 accConfig.odr = BMI160::ACC_ODR_9; //output data rate
EmreE 0:4b4fbf5daa1c 135 if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 136 {
EmreE 0:4b4fbf5daa1c 137 pc.printf("ACC Range = %d\r\n", accConfig.range);
EmreE 0:4b4fbf5daa1c 138 pc.printf("ACC UnderSampling = %d\r\n", accConfig.us);
EmreE 0:4b4fbf5daa1c 139 pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
EmreE 0:4b4fbf5daa1c 140 pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr);
EmreE 0:4b4fbf5daa1c 141 }
EmreE 0:4b4fbf5daa1c 142 else
EmreE 0:4b4fbf5daa1c 143 {
EmreE 0:4b4fbf5daa1c 144 pc.printf("Failed to set accelerometer configuration\r\n");
EmreE 0:4b4fbf5daa1c 145 failures++;
EmreE 0:4b4fbf5daa1c 146 }
EmreE 0:4b4fbf5daa1c 147
EmreE 0:4b4fbf5daa1c 148 BMI160::GyroConfig gyroConfig;
EmreE 0:4b4fbf5daa1c 149 if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 150 {
EmreE 0:4b4fbf5daa1c 151 pc.printf("GYRO Range = %d\r\n", gyroConfig.range);
EmreE 0:4b4fbf5daa1c 152 pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
EmreE 0:4b4fbf5daa1c 153 pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
bjbance 5:f6727800e43f 154
bjbance 5:f6727800e43f 155
EmreE 0:4b4fbf5daa1c 156 }
EmreE 0:4b4fbf5daa1c 157 else
EmreE 0:4b4fbf5daa1c 158 {
EmreE 0:4b4fbf5daa1c 159 pc.printf("Failed to get gyroscope configuration\r\n");
EmreE 0:4b4fbf5daa1c 160 failures++;
EmreE 0:4b4fbf5daa1c 161 }
EmreE 0:4b4fbf5daa1c 162
EmreE 0:4b4fbf5daa1c 163 wait(1.0);
EmreE 0:4b4fbf5daa1c 164 pc.printf("\033[H"); //home
EmreE 0:4b4fbf5daa1c 165 pc.printf("\033[0J"); //erase from cursor to end of screen
EmreE 0:4b4fbf5daa1c 166
EmreE 0:4b4fbf5daa1c 167 if(failures == 0)
EmreE 0:4b4fbf5daa1c 168 {
CharlesMaxim 4:fc8ef84a7dbc 169
CharlesMaxim 4:fc8ef84a7dbc 170
EmreE 0:4b4fbf5daa1c 171 float imuTemperature;
CharlesMaxim 4:fc8ef84a7dbc 172
CharlesMaxim 4:fc8ef84a7dbc 173 double xDeviation, yDeviation, zDeviation;
CharlesMaxim 4:fc8ef84a7dbc 174 double prevGyroX, prevGyroY, prevGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 175 double currentGyroX, currentGyroY, currentGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 176 double diffGyroX, diffGyroY, diffGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 177
CharlesMaxim 4:fc8ef84a7dbc 178 double xDisplacement, yDisplacement, zDisplacement;
CharlesMaxim 4:fc8ef84a7dbc 179 double currentAccX, currentAccY, currentAccZ;
CharlesMaxim 4:fc8ef84a7dbc 180 double prevAccX, prevAccY, prevAccZ;
CharlesMaxim 4:fc8ef84a7dbc 181 double diffAccX, diffAccY, diffAccZ;
CharlesMaxim 4:fc8ef84a7dbc 182
CharlesMaxim 4:fc8ef84a7dbc 183 double xVelocity, yVelocity, zVelocity;
CharlesMaxim 4:fc8ef84a7dbc 184 double timeDiff, time_1, time_2;
CharlesMaxim 4:fc8ef84a7dbc 185 bool timeFlag = false;
EmreE 0:4b4fbf5daa1c 186 BMI160::SensorData accData;
EmreE 0:4b4fbf5daa1c 187 BMI160::SensorData gyroData;
EmreE 0:4b4fbf5daa1c 188 BMI160::SensorTime sensorTime;
CharlesMaxim 4:fc8ef84a7dbc 189
CharlesMaxim 4:fc8ef84a7dbc 190 //PwmPin = 1;
bjbance 5:f6727800e43f 191 float apitch = 0;
bjbance 5:f6727800e43f 192 float k = 0.6;
bjbance 5:f6727800e43f 193
bjbance 5:f6727800e43f 194 time_1 = sensorTime.seconds;
EmreE 0:4b4fbf5daa1c 195 while(1)
EmreE 0:4b4fbf5daa1c 196 {
CharlesMaxim 4:fc8ef84a7dbc 197
EmreE 0:4b4fbf5daa1c 198 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
EmreE 0:4b4fbf5daa1c 199 imu.getTemperature(&imuTemperature);
CharlesMaxim 4:fc8ef84a7dbc 200
bjbance 5:f6727800e43f 201 time_2 = sensorTime.seconds;
bjbance 5:f6727800e43f 202 apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
bjbance 5:f6727800e43f 203 daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch);
bjbance 5:f6727800e43f 204 time_1 = time_2;
bjbance 5:f6727800e43f 205
CharlesMaxim 4:fc8ef84a7dbc 206 //printRegister(imu, BMI160::GYR_CONF);
CharlesMaxim 4:fc8ef84a7dbc 207
bjbance 5:f6727800e43f 208 }
bjbance 5:f6727800e43f 209 }
bjbance 5:f6727800e43f 210 }
bjbance 5:f6727800e43f 211
bjbance 5:f6727800e43f 212 /*
CharlesMaxim 4:fc8ef84a7dbc 213 if(timeFlag == true){
CharlesMaxim 4:fc8ef84a7dbc 214
CharlesMaxim 4:fc8ef84a7dbc 215 currentGyroX = gyroData.xAxis.scaled;
CharlesMaxim 4:fc8ef84a7dbc 216 currentAccX = accData.xAxis.scaled;
EmreE 0:4b4fbf5daa1c 217
CharlesMaxim 4:fc8ef84a7dbc 218
CharlesMaxim 4:fc8ef84a7dbc 219 diffGyroX = abs(currentGyroX - prevGyroX);
CharlesMaxim 4:fc8ef84a7dbc 220 diffAccX = abs(currentAccX - prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 221
CharlesMaxim 4:fc8ef84a7dbc 222 time_2 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 223 timeDiff = time_2 - time_1;
CharlesMaxim 4:fc8ef84a7dbc 224
CharlesMaxim 4:fc8ef84a7dbc 225 if (diffGyroX > 2){
CharlesMaxim 4:fc8ef84a7dbc 226 xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff));
CharlesMaxim 4:fc8ef84a7dbc 227 }
CharlesMaxim 4:fc8ef84a7dbc 228 if (diffAccX > 0.009){
CharlesMaxim 4:fc8ef84a7dbc 229 xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 230 xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 231 }
CharlesMaxim 4:fc8ef84a7dbc 232
CharlesMaxim 4:fc8ef84a7dbc 233 // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 234
CharlesMaxim 4:fc8ef84a7dbc 235 //control motor for proportional linearity
CharlesMaxim 4:fc8ef84a7dbc 236
CharlesMaxim 4:fc8ef84a7dbc 237 if(xDeviation < 0.0)
CharlesMaxim 4:fc8ef84a7dbc 238 {
CharlesMaxim 4:fc8ef84a7dbc 239 M_1 = 0;
CharlesMaxim 4:fc8ef84a7dbc 240 M_2 = 1;
bjbance 5:f6727800e43f 241 daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 242 }
CharlesMaxim 4:fc8ef84a7dbc 243 else{
CharlesMaxim 4:fc8ef84a7dbc 244 M_1 = 1;
CharlesMaxim 4:fc8ef84a7dbc 245 M_2 = 0;
bjbance 5:f6727800e43f 246 daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 247 }
CharlesMaxim 4:fc8ef84a7dbc 248
CharlesMaxim 4:fc8ef84a7dbc 249 //------------------------
CharlesMaxim 4:fc8ef84a7dbc 250
CharlesMaxim 4:fc8ef84a7dbc 251
CharlesMaxim 4:fc8ef84a7dbc 252 //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 253 // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 254 //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement);
EmreE 0:4b4fbf5daa1c 255
CharlesMaxim 4:fc8ef84a7dbc 256 prevGyroX = currentGyroX;
CharlesMaxim 4:fc8ef84a7dbc 257 prevAccX = currentAccX;
CharlesMaxim 4:fc8ef84a7dbc 258 time_1 = time_2;
CharlesMaxim 4:fc8ef84a7dbc 259 }
CharlesMaxim 4:fc8ef84a7dbc 260 else{
CharlesMaxim 4:fc8ef84a7dbc 261 time_1 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 262 timeFlag = true;
CharlesMaxim 4:fc8ef84a7dbc 263 }
CharlesMaxim 4:fc8ef84a7dbc 264
CharlesMaxim 4:fc8ef84a7dbc 265
EmreE 0:4b4fbf5daa1c 266 gLED = !gLED;
CharlesMaxim 4:fc8ef84a7dbc 267 wait_ms(1);
EmreE 0:4b4fbf5daa1c 268 }
EmreE 0:4b4fbf5daa1c 269 }
EmreE 0:4b4fbf5daa1c 270 else
EmreE 0:4b4fbf5daa1c 271 {
EmreE 0:4b4fbf5daa1c 272 while(1)
EmreE 0:4b4fbf5daa1c 273 {
EmreE 0:4b4fbf5daa1c 274 rLED = !rLED;
CharlesMaxim 4:fc8ef84a7dbc 275 wait(0.6);
EmreE 0:4b4fbf5daa1c 276 }
EmreE 0:4b4fbf5daa1c 277 }
bjbance 5:f6727800e43f 278 */
bjbance 5:f6727800e43f 279
bjbance 5:f6727800e43f 280
EmreE 0:4b4fbf5daa1c 281
EmreE 0:4b4fbf5daa1c 282
EmreE 0:4b4fbf5daa1c 283 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 284 void dumpImuRegisters(BMI160 &imu)
EmreE 0:4b4fbf5daa1c 285 {
EmreE 0:4b4fbf5daa1c 286 printRegister(imu, BMI160::CHIP_ID);
EmreE 0:4b4fbf5daa1c 287 printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
EmreE 0:4b4fbf5daa1c 288 printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
EmreE 0:4b4fbf5daa1c 289 printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
EmreE 0:4b4fbf5daa1c 290 printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
EmreE 0:4b4fbf5daa1c 291 printRegister(imu, BMI160::CMD);
EmreE 0:4b4fbf5daa1c 292 pc.printf("\r\n");
EmreE 0:4b4fbf5daa1c 293 }
EmreE 0:4b4fbf5daa1c 294
EmreE 0:4b4fbf5daa1c 295
EmreE 0:4b4fbf5daa1c 296 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 297 void printRegister(BMI160 &imu, BMI160::Registers reg)
EmreE 0:4b4fbf5daa1c 298 {
EmreE 0:4b4fbf5daa1c 299 uint8_t data;
EmreE 0:4b4fbf5daa1c 300 if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 301 {
EmreE 0:4b4fbf5daa1c 302 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
CharlesMaxim 4:fc8ef84a7dbc 303 daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
EmreE 0:4b4fbf5daa1c 304 }
EmreE 0:4b4fbf5daa1c 305 else
EmreE 0:4b4fbf5daa1c 306 {
EmreE 0:4b4fbf5daa1c 307 pc.printf("Failed to read register\r\n");
EmreE 0:4b4fbf5daa1c 308 }
EmreE 0:4b4fbf5daa1c 309 }
EmreE 0:4b4fbf5daa1c 310
CharlesMaxim 4:fc8ef84a7dbc 311 //*****************************************************************************
CharlesMaxim 4:fc8ef84a7dbc 312 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
CharlesMaxim 4:fc8ef84a7dbc 313 {
CharlesMaxim 4:fc8ef84a7dbc 314 imu.writeRegister(reg, data);
CharlesMaxim 4:fc8ef84a7dbc 315
CharlesMaxim 4:fc8ef84a7dbc 316
CharlesMaxim 4:fc8ef84a7dbc 317 }
CharlesMaxim 4:fc8ef84a7dbc 318
EmreE 0:4b4fbf5daa1c 319
EmreE 0:4b4fbf5daa1c 320 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 321 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
EmreE 0:4b4fbf5daa1c 322 {
EmreE 0:4b4fbf5daa1c 323 uint8_t numBytes = ((stopReg - startReg) + 1);
Emre.Eken 2:ba0a55e05168 324 uint8_t buff[32];
EmreE 0:4b4fbf5daa1c 325 uint8_t offset = static_cast<uint8_t>(startReg);
EmreE 0:4b4fbf5daa1c 326
EmreE 0:4b4fbf5daa1c 327 if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 328 {
EmreE 0:4b4fbf5daa1c 329 for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
EmreE 0:4b4fbf5daa1c 330 {
EmreE 0:4b4fbf5daa1c 331 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
EmreE 0:4b4fbf5daa1c 332 }
EmreE 0:4b4fbf5daa1c 333 }
EmreE 0:4b4fbf5daa1c 334 else
EmreE 0:4b4fbf5daa1c 335 {
EmreE 0:4b4fbf5daa1c 336 pc.printf("Failed to read block\r\n");
EmreE 0:4b4fbf5daa1c 337 }
EmreE 0:4b4fbf5daa1c 338 }
CharlesMaxim 4:fc8ef84a7dbc 339
CharlesMaxim 4:fc8ef84a7dbc 340
CharlesMaxim 4:fc8ef84a7dbc 341 /* An example for configuring FOC for accel and gyro data */