![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
New Self Balancing Code
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
main.cpp@0:4b4fbf5daa1c, 2018-04-25 (annotated)
- Committer:
- EmreE
- Date:
- Wed Apr 25 11:00:47 2018 +0000
- Revision:
- 0:4b4fbf5daa1c
- Child:
- 2:ba0a55e05168
First Revision
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" |
EmreE | 0:4b4fbf5daa1c | 36 | #include "max32630hsp.h" |
EmreE | 0:4b4fbf5daa1c | 37 | |
EmreE | 0:4b4fbf5daa1c | 38 | MAX32630HSP icarus(MAX32630HSP::VIO_3V3); |
EmreE | 0:4b4fbf5daa1c | 39 | |
EmreE | 0:4b4fbf5daa1c | 40 | DigitalOut rLED(LED1, LED_OFF); |
EmreE | 0:4b4fbf5daa1c | 41 | DigitalOut gLED(LED2, LED_OFF); |
EmreE | 0:4b4fbf5daa1c | 42 | DigitalOut bLED(LED3, LED_OFF); |
EmreE | 0:4b4fbf5daa1c | 43 | |
EmreE | 0:4b4fbf5daa1c | 44 | Serial pc(USBTX,USBRX); |
EmreE | 0:4b4fbf5daa1c | 45 | |
EmreE | 0:4b4fbf5daa1c | 46 | I2C i2cBus(I2C2_SDA, I2C2_SCL); |
EmreE | 0:4b4fbf5daa1c | 47 | |
EmreE | 0:4b4fbf5daa1c | 48 | BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); |
EmreE | 0:4b4fbf5daa1c | 49 | |
EmreE | 0:4b4fbf5daa1c | 50 | void dumpImuRegisters(BMI160 &imu); |
EmreE | 0:4b4fbf5daa1c | 51 | void printRegister(BMI160 &imu, BMI160::Registers reg); |
EmreE | 0:4b4fbf5daa1c | 52 | void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg); |
EmreE | 0:4b4fbf5daa1c | 53 | |
EmreE | 0:4b4fbf5daa1c | 54 | int main() |
EmreE | 0:4b4fbf5daa1c | 55 | { |
EmreE | 0:4b4fbf5daa1c | 56 | i2cBus.frequency(400000); |
EmreE | 0:4b4fbf5daa1c | 57 | |
EmreE | 0:4b4fbf5daa1c | 58 | pc.printf("\033[H"); //home |
EmreE | 0:4b4fbf5daa1c | 59 | pc.printf("\033[0J"); //erase from cursor to end of screen |
EmreE | 0:4b4fbf5daa1c | 60 | |
EmreE | 0:4b4fbf5daa1c | 61 | uint32_t failures = 0; |
EmreE | 0:4b4fbf5daa1c | 62 | |
EmreE | 0:4b4fbf5daa1c | 63 | if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 64 | { |
EmreE | 0:4b4fbf5daa1c | 65 | pc.printf("Failed to set gyroscope power mode\r\n"); |
EmreE | 0:4b4fbf5daa1c | 66 | failures++; |
EmreE | 0:4b4fbf5daa1c | 67 | } |
EmreE | 0:4b4fbf5daa1c | 68 | wait_ms(100); |
EmreE | 0:4b4fbf5daa1c | 69 | |
EmreE | 0:4b4fbf5daa1c | 70 | if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 71 | { |
EmreE | 0:4b4fbf5daa1c | 72 | pc.printf("Failed to set accelerometer power mode\r\n"); |
EmreE | 0:4b4fbf5daa1c | 73 | failures++; |
EmreE | 0:4b4fbf5daa1c | 74 | } |
EmreE | 0:4b4fbf5daa1c | 75 | wait_ms(100); |
EmreE | 0:4b4fbf5daa1c | 76 | |
EmreE | 0:4b4fbf5daa1c | 77 | |
EmreE | 0:4b4fbf5daa1c | 78 | BMI160::AccConfig accConfig; |
EmreE | 0:4b4fbf5daa1c | 79 | //example of using getSensorConfig |
EmreE | 0:4b4fbf5daa1c | 80 | if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 81 | { |
EmreE | 0:4b4fbf5daa1c | 82 | pc.printf("ACC Range = %d\r\n", accConfig.range); |
EmreE | 0:4b4fbf5daa1c | 83 | pc.printf("ACC UnderSampling = %d\r\n", accConfig.us); |
EmreE | 0:4b4fbf5daa1c | 84 | pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); |
EmreE | 0:4b4fbf5daa1c | 85 | pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); |
EmreE | 0:4b4fbf5daa1c | 86 | } |
EmreE | 0:4b4fbf5daa1c | 87 | else |
EmreE | 0:4b4fbf5daa1c | 88 | { |
EmreE | 0:4b4fbf5daa1c | 89 | pc.printf("Failed to get accelerometer configuration\r\n"); |
EmreE | 0:4b4fbf5daa1c | 90 | failures++; |
EmreE | 0:4b4fbf5daa1c | 91 | } |
EmreE | 0:4b4fbf5daa1c | 92 | |
EmreE | 0:4b4fbf5daa1c | 93 | //example of setting user defined configuration |
EmreE | 0:4b4fbf5daa1c | 94 | accConfig.range = BMI160::SENS_4G; |
EmreE | 0:4b4fbf5daa1c | 95 | accConfig.us = BMI160::ACC_US_OFF; |
EmreE | 0:4b4fbf5daa1c | 96 | accConfig.bwp = BMI160::ACC_BWP_2; |
EmreE | 0:4b4fbf5daa1c | 97 | accConfig.odr = BMI160::ACC_ODR_8; |
EmreE | 0:4b4fbf5daa1c | 98 | if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 99 | { |
EmreE | 0:4b4fbf5daa1c | 100 | pc.printf("ACC Range = %d\r\n", accConfig.range); |
EmreE | 0:4b4fbf5daa1c | 101 | pc.printf("ACC UnderSampling = %d\r\n", accConfig.us); |
EmreE | 0:4b4fbf5daa1c | 102 | pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); |
EmreE | 0:4b4fbf5daa1c | 103 | pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); |
EmreE | 0:4b4fbf5daa1c | 104 | } |
EmreE | 0:4b4fbf5daa1c | 105 | else |
EmreE | 0:4b4fbf5daa1c | 106 | { |
EmreE | 0:4b4fbf5daa1c | 107 | pc.printf("Failed to set accelerometer configuration\r\n"); |
EmreE | 0:4b4fbf5daa1c | 108 | failures++; |
EmreE | 0:4b4fbf5daa1c | 109 | } |
EmreE | 0:4b4fbf5daa1c | 110 | |
EmreE | 0:4b4fbf5daa1c | 111 | BMI160::GyroConfig gyroConfig; |
EmreE | 0:4b4fbf5daa1c | 112 | if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 113 | { |
EmreE | 0:4b4fbf5daa1c | 114 | pc.printf("GYRO Range = %d\r\n", gyroConfig.range); |
EmreE | 0:4b4fbf5daa1c | 115 | pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp); |
EmreE | 0:4b4fbf5daa1c | 116 | pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr); |
EmreE | 0:4b4fbf5daa1c | 117 | } |
EmreE | 0:4b4fbf5daa1c | 118 | else |
EmreE | 0:4b4fbf5daa1c | 119 | { |
EmreE | 0:4b4fbf5daa1c | 120 | pc.printf("Failed to get gyroscope configuration\r\n"); |
EmreE | 0:4b4fbf5daa1c | 121 | failures++; |
EmreE | 0:4b4fbf5daa1c | 122 | } |
EmreE | 0:4b4fbf5daa1c | 123 | |
EmreE | 0:4b4fbf5daa1c | 124 | wait(1.0); |
EmreE | 0:4b4fbf5daa1c | 125 | pc.printf("\033[H"); //home |
EmreE | 0:4b4fbf5daa1c | 126 | pc.printf("\033[0J"); //erase from cursor to end of screen |
EmreE | 0:4b4fbf5daa1c | 127 | |
EmreE | 0:4b4fbf5daa1c | 128 | if(failures == 0) |
EmreE | 0:4b4fbf5daa1c | 129 | { |
EmreE | 0:4b4fbf5daa1c | 130 | float imuTemperature; |
EmreE | 0:4b4fbf5daa1c | 131 | BMI160::SensorData accData; |
EmreE | 0:4b4fbf5daa1c | 132 | BMI160::SensorData gyroData; |
EmreE | 0:4b4fbf5daa1c | 133 | BMI160::SensorTime sensorTime; |
EmreE | 0:4b4fbf5daa1c | 134 | |
EmreE | 0:4b4fbf5daa1c | 135 | while(1) |
EmreE | 0:4b4fbf5daa1c | 136 | { |
EmreE | 0:4b4fbf5daa1c | 137 | imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); |
EmreE | 0:4b4fbf5daa1c | 138 | imu.getTemperature(&imuTemperature); |
EmreE | 0:4b4fbf5daa1c | 139 | |
EmreE | 0:4b4fbf5daa1c | 140 | pc.printf("ACC xAxis = %s%4.3f\r\n", "\033[K", accData.xAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 141 | pc.printf("ACC yAxis = %s%4.3f\r\n", "\033[K", accData.yAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 142 | pc.printf("ACC zAxis = %s%4.3f\r\n\r\n", "\033[K", accData.zAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 143 | |
EmreE | 0:4b4fbf5daa1c | 144 | pc.printf("GYRO xAxis = %s%5.1f\r\n", "\033[K", gyroData.xAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 145 | pc.printf("GYRO yAxis = %s%5.1f\r\n", "\033[K", gyroData.yAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 146 | pc.printf("GYRO zAxis = %s%5.1f\r\n\r\n", "\033[K", gyroData.zAxis.scaled); |
EmreE | 0:4b4fbf5daa1c | 147 | |
EmreE | 0:4b4fbf5daa1c | 148 | pc.printf("Sensor Time = %s%f\r\n", "\033[K", sensorTime.seconds); |
EmreE | 0:4b4fbf5daa1c | 149 | pc.printf("Sensor Temperature = %s%5.3f\r\n", "\033[K", imuTemperature); |
EmreE | 0:4b4fbf5daa1c | 150 | |
EmreE | 0:4b4fbf5daa1c | 151 | pc.printf("\033[H"); //home |
EmreE | 0:4b4fbf5daa1c | 152 | gLED = !gLED; |
EmreE | 0:4b4fbf5daa1c | 153 | wait(0.5); |
EmreE | 0:4b4fbf5daa1c | 154 | } |
EmreE | 0:4b4fbf5daa1c | 155 | } |
EmreE | 0:4b4fbf5daa1c | 156 | else |
EmreE | 0:4b4fbf5daa1c | 157 | { |
EmreE | 0:4b4fbf5daa1c | 158 | while(1) |
EmreE | 0:4b4fbf5daa1c | 159 | { |
EmreE | 0:4b4fbf5daa1c | 160 | rLED = !rLED; |
EmreE | 0:4b4fbf5daa1c | 161 | wait(0.25); |
EmreE | 0:4b4fbf5daa1c | 162 | } |
EmreE | 0:4b4fbf5daa1c | 163 | } |
EmreE | 0:4b4fbf5daa1c | 164 | } |
EmreE | 0:4b4fbf5daa1c | 165 | |
EmreE | 0:4b4fbf5daa1c | 166 | |
EmreE | 0:4b4fbf5daa1c | 167 | //***************************************************************************** |
EmreE | 0:4b4fbf5daa1c | 168 | void dumpImuRegisters(BMI160 &imu) |
EmreE | 0:4b4fbf5daa1c | 169 | { |
EmreE | 0:4b4fbf5daa1c | 170 | printRegister(imu, BMI160::CHIP_ID); |
EmreE | 0:4b4fbf5daa1c | 171 | printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA); |
EmreE | 0:4b4fbf5daa1c | 172 | printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1); |
EmreE | 0:4b4fbf5daa1c | 173 | printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST); |
EmreE | 0:4b4fbf5daa1c | 174 | printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1); |
EmreE | 0:4b4fbf5daa1c | 175 | printRegister(imu, BMI160::CMD); |
EmreE | 0:4b4fbf5daa1c | 176 | pc.printf("\r\n"); |
EmreE | 0:4b4fbf5daa1c | 177 | } |
EmreE | 0:4b4fbf5daa1c | 178 | |
EmreE | 0:4b4fbf5daa1c | 179 | |
EmreE | 0:4b4fbf5daa1c | 180 | //***************************************************************************** |
EmreE | 0:4b4fbf5daa1c | 181 | void printRegister(BMI160 &imu, BMI160::Registers reg) |
EmreE | 0:4b4fbf5daa1c | 182 | { |
EmreE | 0:4b4fbf5daa1c | 183 | uint8_t data; |
EmreE | 0:4b4fbf5daa1c | 184 | if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 185 | { |
EmreE | 0:4b4fbf5daa1c | 186 | pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); |
EmreE | 0:4b4fbf5daa1c | 187 | } |
EmreE | 0:4b4fbf5daa1c | 188 | else |
EmreE | 0:4b4fbf5daa1c | 189 | { |
EmreE | 0:4b4fbf5daa1c | 190 | pc.printf("Failed to read register\r\n"); |
EmreE | 0:4b4fbf5daa1c | 191 | } |
EmreE | 0:4b4fbf5daa1c | 192 | } |
EmreE | 0:4b4fbf5daa1c | 193 | |
EmreE | 0:4b4fbf5daa1c | 194 | |
EmreE | 0:4b4fbf5daa1c | 195 | //***************************************************************************** |
EmreE | 0:4b4fbf5daa1c | 196 | void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) |
EmreE | 0:4b4fbf5daa1c | 197 | { |
EmreE | 0:4b4fbf5daa1c | 198 | uint8_t numBytes = ((stopReg - startReg) + 1); |
EmreE | 0:4b4fbf5daa1c | 199 | uint8_t buff[numBytes]; |
EmreE | 0:4b4fbf5daa1c | 200 | uint8_t offset = static_cast<uint8_t>(startReg); |
EmreE | 0:4b4fbf5daa1c | 201 | |
EmreE | 0:4b4fbf5daa1c | 202 | if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) |
EmreE | 0:4b4fbf5daa1c | 203 | { |
EmreE | 0:4b4fbf5daa1c | 204 | for(uint8_t idx = offset; idx < (numBytes + offset); idx++) |
EmreE | 0:4b4fbf5daa1c | 205 | { |
EmreE | 0:4b4fbf5daa1c | 206 | pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]); |
EmreE | 0:4b4fbf5daa1c | 207 | } |
EmreE | 0:4b4fbf5daa1c | 208 | } |
EmreE | 0:4b4fbf5daa1c | 209 | else |
EmreE | 0:4b4fbf5daa1c | 210 | { |
EmreE | 0:4b4fbf5daa1c | 211 | pc.printf("Failed to read block\r\n"); |
EmreE | 0:4b4fbf5daa1c | 212 | } |
EmreE | 0:4b4fbf5daa1c | 213 | } |