TSDA Robotics / Mbed 2 deprecated MAX32630HSP3_Pitch_Charles

New Self Balancing Code

Dependencies:   mbed BMI160 max32630fthr_pitch USBDevice Math

main.cpp@7:2c48702d99e3, 2018-12-21 (annotated)

Committer:
CharlesMaxim
Date:
Fri Dec 21 19:49:34 2018 +0000
Revision:
7:2c48702d99e3
Parent:
5:f6727800e43f 
Updated Self Balancing

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 7:2c48702d99e3 51 PwmOut PWM(P3_5);
CharlesMaxim 7:2c48702d99e3 52 DigitalOut Dir(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 7:2c48702d99e3 172 float dutyCycle;
CharlesMaxim 4:fc8ef84a7dbc 173
CharlesMaxim 4:fc8ef84a7dbc 174 double xDeviation, yDeviation, zDeviation;
CharlesMaxim 4:fc8ef84a7dbc 175 double prevGyroX, prevGyroY, prevGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 176 double currentGyroX, currentGyroY, currentGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 177 double diffGyroX, diffGyroY, diffGyroZ;
CharlesMaxim 4:fc8ef84a7dbc 178
CharlesMaxim 4:fc8ef84a7dbc 179 double xDisplacement, yDisplacement, zDisplacement;
CharlesMaxim 4:fc8ef84a7dbc 180 double currentAccX, currentAccY, currentAccZ;
CharlesMaxim 4:fc8ef84a7dbc 181 double prevAccX, prevAccY, prevAccZ;
CharlesMaxim 4:fc8ef84a7dbc 182 double diffAccX, diffAccY, diffAccZ;
CharlesMaxim 4:fc8ef84a7dbc 183
CharlesMaxim 4:fc8ef84a7dbc 184 double xVelocity, yVelocity, zVelocity;
CharlesMaxim 4:fc8ef84a7dbc 185 double timeDiff, time_1, time_2;
CharlesMaxim 4:fc8ef84a7dbc 186 bool timeFlag = false;
EmreE 0:4b4fbf5daa1c 187 BMI160::SensorData accData;
EmreE 0:4b4fbf5daa1c 188 BMI160::SensorData gyroData;
EmreE 0:4b4fbf5daa1c 189 BMI160::SensorTime sensorTime;
CharlesMaxim 4:fc8ef84a7dbc 190
CharlesMaxim 4:fc8ef84a7dbc 191 //PwmPin = 1;
bjbance 5:f6727800e43f 192 float apitch = 0;
bjbance 5:f6727800e43f 193 float k = 0.6;
bjbance 5:f6727800e43f 194
bjbance 5:f6727800e43f 195 time_1 = sensorTime.seconds;
EmreE 0:4b4fbf5daa1c 196 while(1)
EmreE 0:4b4fbf5daa1c 197 {
CharlesMaxim 4:fc8ef84a7dbc 198
EmreE 0:4b4fbf5daa1c 199 imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
EmreE 0:4b4fbf5daa1c 200 imu.getTemperature(&imuTemperature);
CharlesMaxim 4:fc8ef84a7dbc 201
bjbance 5:f6727800e43f 202 time_2 = sensorTime.seconds;
bjbance 5:f6727800e43f 203 apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
CharlesMaxim 7:2c48702d99e3 204 pc.printf("Drift: %s%4.3f\r\n", "\033[K", apitch);
CharlesMaxim 7:2c48702d99e3 205
CharlesMaxim 7:2c48702d99e3 206
CharlesMaxim 7:2c48702d99e3 207 //dutyCycle = (apitch * (0.2 / 10)) + 0.8;
CharlesMaxim 7:2c48702d99e3 208 // dutyCycle = fabsf(dutyCycle);
CharlesMaxim 7:2c48702d99e3 209 // pc.printf("PWM: %s%4.3f\r\n", "\033[K", dutyCycle);
CharlesMaxim 7:2c48702d99e3 210
CharlesMaxim 7:2c48702d99e3 211 time_1 = time_2;
CharlesMaxim 7:2c48702d99e3 212 /*
CharlesMaxim 7:2c48702d99e3 213 if(dutyCycle < 0 ){
CharlesMaxim 7:2c48702d99e3 214 dutyCycle = 0 - dutyCycle;
CharlesMaxim 7:2c48702d99e3 215 }
CharlesMaxim 7:2c48702d99e3 216 */
CharlesMaxim 7:2c48702d99e3 217 if(apitch <= 0){
CharlesMaxim 7:2c48702d99e3 218 PWM = 0.5;
CharlesMaxim 7:2c48702d99e3 219 Dir = 1;
CharlesMaxim 7:2c48702d99e3 220 }
CharlesMaxim 7:2c48702d99e3 221
CharlesMaxim 7:2c48702d99e3 222 if(apitch > 0){
CharlesMaxim 7:2c48702d99e3 223 PWM = 0.5;
CharlesMaxim 7:2c48702d99e3 224 Dir = 0;
CharlesMaxim 7:2c48702d99e3 225 }
bjbance 5:f6727800e43f 226
CharlesMaxim 4:fc8ef84a7dbc 227 //printRegister(imu, BMI160::GYR_CONF);
CharlesMaxim 4:fc8ef84a7dbc 228
bjbance 5:f6727800e43f 229 }
bjbance 5:f6727800e43f 230 }
bjbance 5:f6727800e43f 231 }
bjbance 5:f6727800e43f 232
bjbance 5:f6727800e43f 233 /*
CharlesMaxim 4:fc8ef84a7dbc 234 if(timeFlag == true){
CharlesMaxim 4:fc8ef84a7dbc 235
CharlesMaxim 4:fc8ef84a7dbc 236 currentGyroX = gyroData.xAxis.scaled;
CharlesMaxim 4:fc8ef84a7dbc 237 currentAccX = accData.xAxis.scaled;
EmreE 0:4b4fbf5daa1c 238
CharlesMaxim 4:fc8ef84a7dbc 239
CharlesMaxim 4:fc8ef84a7dbc 240 diffGyroX = abs(currentGyroX - prevGyroX);
CharlesMaxim 4:fc8ef84a7dbc 241 diffAccX = abs(currentAccX - prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 242
CharlesMaxim 4:fc8ef84a7dbc 243 time_2 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 244 timeDiff = time_2 - time_1;
CharlesMaxim 4:fc8ef84a7dbc 245
CharlesMaxim 4:fc8ef84a7dbc 246 if (diffGyroX > 2){
CharlesMaxim 4:fc8ef84a7dbc 247 xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff));
CharlesMaxim 4:fc8ef84a7dbc 248 }
CharlesMaxim 4:fc8ef84a7dbc 249 if (diffAccX > 0.009){
CharlesMaxim 4:fc8ef84a7dbc 250 xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 251 xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 252 }
CharlesMaxim 4:fc8ef84a7dbc 253
CharlesMaxim 4:fc8ef84a7dbc 254 // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 255
CharlesMaxim 4:fc8ef84a7dbc 256 //control motor for proportional linearity
CharlesMaxim 4:fc8ef84a7dbc 257
CharlesMaxim 4:fc8ef84a7dbc 258 if(xDeviation < 0.0)
CharlesMaxim 4:fc8ef84a7dbc 259 {
CharlesMaxim 4:fc8ef84a7dbc 260 M_1 = 0;
CharlesMaxim 4:fc8ef84a7dbc 261 M_2 = 1;
bjbance 5:f6727800e43f 262 daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 263 }
CharlesMaxim 4:fc8ef84a7dbc 264 else{
CharlesMaxim 4:fc8ef84a7dbc 265 M_1 = 1;
CharlesMaxim 4:fc8ef84a7dbc 266 M_2 = 0;
bjbance 5:f6727800e43f 267 daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
CharlesMaxim 4:fc8ef84a7dbc 268 }
CharlesMaxim 4:fc8ef84a7dbc 269
CharlesMaxim 4:fc8ef84a7dbc 270 //------------------------
CharlesMaxim 4:fc8ef84a7dbc 271
CharlesMaxim 4:fc8ef84a7dbc 272
CharlesMaxim 4:fc8ef84a7dbc 273 //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX);
CharlesMaxim 4:fc8ef84a7dbc 274 // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff);
CharlesMaxim 4:fc8ef84a7dbc 275 //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement);
EmreE 0:4b4fbf5daa1c 276
CharlesMaxim 4:fc8ef84a7dbc 277 prevGyroX = currentGyroX;
CharlesMaxim 4:fc8ef84a7dbc 278 prevAccX = currentAccX;
CharlesMaxim 4:fc8ef84a7dbc 279 time_1 = time_2;
CharlesMaxim 4:fc8ef84a7dbc 280 }
CharlesMaxim 4:fc8ef84a7dbc 281 else{
CharlesMaxim 4:fc8ef84a7dbc 282 time_1 = sensorTime.seconds;
CharlesMaxim 4:fc8ef84a7dbc 283 timeFlag = true;
CharlesMaxim 4:fc8ef84a7dbc 284 }
CharlesMaxim 4:fc8ef84a7dbc 285
CharlesMaxim 4:fc8ef84a7dbc 286
EmreE 0:4b4fbf5daa1c 287 gLED = !gLED;
CharlesMaxim 4:fc8ef84a7dbc 288 wait_ms(1);
EmreE 0:4b4fbf5daa1c 289 }
EmreE 0:4b4fbf5daa1c 290 }
EmreE 0:4b4fbf5daa1c 291 else
EmreE 0:4b4fbf5daa1c 292 {
EmreE 0:4b4fbf5daa1c 293 while(1)
EmreE 0:4b4fbf5daa1c 294 {
EmreE 0:4b4fbf5daa1c 295 rLED = !rLED;
CharlesMaxim 4:fc8ef84a7dbc 296 wait(0.6);
EmreE 0:4b4fbf5daa1c 297 }
EmreE 0:4b4fbf5daa1c 298 }
bjbance 5:f6727800e43f 299 */
bjbance 5:f6727800e43f 300
bjbance 5:f6727800e43f 301
EmreE 0:4b4fbf5daa1c 302
EmreE 0:4b4fbf5daa1c 303
EmreE 0:4b4fbf5daa1c 304 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 305 void dumpImuRegisters(BMI160 &imu)
EmreE 0:4b4fbf5daa1c 306 {
EmreE 0:4b4fbf5daa1c 307 printRegister(imu, BMI160::CHIP_ID);
EmreE 0:4b4fbf5daa1c 308 printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
EmreE 0:4b4fbf5daa1c 309 printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
EmreE 0:4b4fbf5daa1c 310 printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
EmreE 0:4b4fbf5daa1c 311 printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
EmreE 0:4b4fbf5daa1c 312 printRegister(imu, BMI160::CMD);
EmreE 0:4b4fbf5daa1c 313 pc.printf("\r\n");
EmreE 0:4b4fbf5daa1c 314 }
EmreE 0:4b4fbf5daa1c 315
EmreE 0:4b4fbf5daa1c 316
EmreE 0:4b4fbf5daa1c 317 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 318 void printRegister(BMI160 &imu, BMI160::Registers reg)
EmreE 0:4b4fbf5daa1c 319 {
EmreE 0:4b4fbf5daa1c 320 uint8_t data;
EmreE 0:4b4fbf5daa1c 321 if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 322 {
EmreE 0:4b4fbf5daa1c 323 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
CharlesMaxim 4:fc8ef84a7dbc 324 daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data);
EmreE 0:4b4fbf5daa1c 325 }
EmreE 0:4b4fbf5daa1c 326 else
EmreE 0:4b4fbf5daa1c 327 {
EmreE 0:4b4fbf5daa1c 328 pc.printf("Failed to read register\r\n");
EmreE 0:4b4fbf5daa1c 329 }
EmreE 0:4b4fbf5daa1c 330 }
EmreE 0:4b4fbf5daa1c 331
CharlesMaxim 4:fc8ef84a7dbc 332 //*****************************************************************************
CharlesMaxim 4:fc8ef84a7dbc 333 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data)
CharlesMaxim 4:fc8ef84a7dbc 334 {
CharlesMaxim 4:fc8ef84a7dbc 335 imu.writeRegister(reg, data);
CharlesMaxim 4:fc8ef84a7dbc 336
CharlesMaxim 4:fc8ef84a7dbc 337
CharlesMaxim 4:fc8ef84a7dbc 338 }
CharlesMaxim 4:fc8ef84a7dbc 339
EmreE 0:4b4fbf5daa1c 340
EmreE 0:4b4fbf5daa1c 341 //*****************************************************************************
EmreE 0:4b4fbf5daa1c 342 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
EmreE 0:4b4fbf5daa1c 343 {
EmreE 0:4b4fbf5daa1c 344 uint8_t numBytes = ((stopReg - startReg) + 1);
Emre.Eken 2:ba0a55e05168 345 uint8_t buff[32];
EmreE 0:4b4fbf5daa1c 346 uint8_t offset = static_cast<uint8_t>(startReg);
EmreE 0:4b4fbf5daa1c 347
EmreE 0:4b4fbf5daa1c 348 if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
EmreE 0:4b4fbf5daa1c 349 {
EmreE 0:4b4fbf5daa1c 350 for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
EmreE 0:4b4fbf5daa1c 351 {
EmreE 0:4b4fbf5daa1c 352 pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]);
EmreE 0:4b4fbf5daa1c 353 }
EmreE 0:4b4fbf5daa1c 354 }
EmreE 0:4b4fbf5daa1c 355 else
EmreE 0:4b4fbf5daa1c 356 {
EmreE 0:4b4fbf5daa1c 357 pc.printf("Failed to read block\r\n");
EmreE 0:4b4fbf5daa1c 358 }
EmreE 0:4b4fbf5daa1c 359 }
CharlesMaxim 4:fc8ef84a7dbc 360
CharlesMaxim 4:fc8ef84a7dbc 361
CharlesMaxim 4:fc8ef84a7dbc 362 /* An example for configuring FOC for accel and gyro data */