TSDA Robotics / Mbed 2 deprecated MAX32630HSP3_Pitch_Charles

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?

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"
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 }