New Self Balancing Code
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:2c48702d99e3
pitch correction;
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" |
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 */ |