Final Tree
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
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
- Child:
- 5:f6727800e43f
...
Who changed what in which revision?
User | Revision | Line number | New 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 */ |