Final Tree
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
Revision 7:b33be863fbb5, committed 2019-01-28
- Comitter:
- bjbance
- Date:
- Mon Jan 28 09:00:47 2019 +0000
- Parent:
- 6:ee03dafaa43f
- Commit message:
- Separate library for sensors
Changed in this revision
diff -r ee03dafaa43f -r b33be863fbb5 Sensors/Orientation.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Orientation.cpp Mon Jan 28 09:00:47 2019 +0000 @@ -0,0 +1,191 @@ +#include "mbed.h" +#include "bmi160.h" +#include "max32630fthr.h" +#include "USBSerial.h" +#include "stdlib.h" +#include "math.h" +#include "Orientation.h" + +I2C i2cBus(I2C2_SDA, I2C2_SCL); +BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); + +BMI160::SensorData accData; +BMI160::SensorData gyroData; +BMI160::SensorTime sensorTime; +BMI160::AccConfig accConfig; +BMI160::GyroConfig gyroConfig; +float imuTemperature; + + +Serial pc2(P2_1, P2_0); + +void Orientation::init(){ + pitch = 0; + k = 0.65; + time1 = sensorTime.seconds; + time2 = sensorTime.seconds; + + + i2cBus.frequency(400000); + writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500); + writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13); + writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE ); + wait(0.5); + writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC); + + writeReg(imu, BMI160::CMD, BMI160::FOC_START); + + + pc2.printf("\033[H"); //home + pc2.printf("\033[0J"); //erase from cursor to end of screen + + uint32_t failures = 0; + + if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + pc2.printf("Failed to set gyroscope power mode\r\n"); + failures++; + } + wait_ms(100); + + if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) + { + pc2.printf("Failed to set accelerometer power mode\r\n"); + failures++; + } + wait_ms(100); + + + + //example of using getSensorConfig + if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) + { + pc2.printf("ACC Range = %d\r\n", accConfig.range); + pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us); + pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); + pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); + } + else + { + pc2.printf("Failed to get accelerometer configuration\r\n"); + failures++; + } + + //example of setting user defined configuration + accConfig.range = BMI160::SENS_2G; //rage is 2g + accConfig.us = BMI160::ACC_US_OFF; //undersampling is off + accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles + accConfig.odr = BMI160::ACC_ODR_9; //output data rate + if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) + { + pc2.printf("ACC Range = %d\r\n", accConfig.range); + pc2.printf("ACC UnderSampling = %d\r\n", accConfig.us); + pc2.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); + pc2.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); + } + else + { + pc2.printf("Failed to set accelerometer configuration\r\n"); + failures++; + } + + if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) + { + pc2.printf("GYRO Range = %d\r\n", gyroConfig.range); + pc2.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp); + pc2.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr); + + + } + else + { + pc2.printf("Failed to get gyroscope configuration\r\n"); + failures++; + } + + wait(1.0); + +} + + +float Orientation::getPitch() +{ + return pitch; +} + +void Orientation::updatePitch() +{ + imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); + imu.getTemperature(&imuTemperature); + + time2 = sensorTime.seconds; + pitch = compFilter(k, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time2 - time1); + //daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch); + time1 = time2; +// return pitch; + printRegister(imu, BMI160::GYR_CONF); +} + +//***************************************************************************** +void dumpImuRegisters(BMI160 &imu) +{ + printRegister(imu, BMI160::CHIP_ID); + printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA); + printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1); + printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST); + printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1); + printRegister(imu, BMI160::CMD); +// pc.printf("\r\n"); +} + + +//***************************************************************************** +void printRegister(BMI160 &imu, BMI160::Registers reg) +{ + uint8_t data; + if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) + { +// pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); +// daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); + } + else + { +// pc.printf("Failed to read register\r\n"); + } +} + +//***************************************************************************** +void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data) +{ + imu.writeRegister(reg, data); + + +} + + +//***************************************************************************** +void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) +{ + uint8_t numBytes = ((stopReg - startReg) + 1); + uint8_t buff[32]; + uint8_t offset = static_cast<uint8_t>(startReg); + + if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) + { + for(uint8_t idx = offset; idx < (numBytes + offset); idx++) + { + // pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]); + } + } + else + { +// pc.printf("Failed to read block\r\n"); + } +} + + + +float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT) +{ + return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ)))); +} \ No newline at end of file
diff -r ee03dafaa43f -r b33be863fbb5 Sensors/Orientation.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Orientation.h Mon Jan 28 09:00:47 2019 +0000 @@ -0,0 +1,38 @@ +#include "mbed.h" +#include "bmi160.h" +#include "max32630fthr.h" +#include "stdlib.h" + + + +#include "USBSerial.h" +#include "math.h" + + + +void dumpImuRegisters(BMI160 &imu); +void printRegister(BMI160 &imu, BMI160::Registers reg); +void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg); +void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data); +float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT); + + +class Orientation{ + float pitch, time1, time2; + float k; + + + + + + + +public: + void init(); + float getPitch(); + void updatePitch(); + + +}; + +
diff -r ee03dafaa43f -r b33be863fbb5 main.cpp --- a/main.cpp Thu Dec 13 11:24:42 2018 +0000 +++ b/main.cpp Mon Jan 28 09:00:47 2019 +0000 @@ -29,15 +29,14 @@ * property whatsoever. Maxim Integrated Products, Inc. retains all * ownership rights. **********************************************************************/ - - #include "mbed.h" #include "bmi160.h" -//#include "max32630hsp.h" + #include "max32630fthr.h" #include "USBSerial.h" #include "stdlib.h" #include "math.h" +#include "Orientation.h" //MAX32630HSP icarus(MAX32630HSP::VIO_3V3); MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); @@ -51,291 +50,29 @@ DigitalOut M_1(P3_5); DigitalOut M_2(P3_4); - -USBSerial pc(USBTX,USBRX); - -I2C i2cBus(I2C2_SDA, I2C2_SCL); - - - -BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); - - -void dumpImuRegisters(BMI160 &imu); -void printRegister(BMI160 &imu, BMI160::Registers reg); -void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg); -void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data); -//float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT); - - -float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT) -{ - return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ)))); -} - +Orientation orient1; int main() { - i2cBus.frequency(400000); - writeReg(imu, BMI160::GYR_RANGE, BMI160::DPS_500); - writeReg(imu, BMI160::GYR_CONF, BMI160::GYRO_ODR_13); - writeReg(imu, BMI160::FOC_CONF, BMI160::FOC_VALUE ); - wait(0.5); - writeReg(imu, BMI160::OFFSET_6, BMI160::FOC_ENABLE_GYR_ACC); gLED = 1; bLED = 0; wait(1); - writeReg(imu, BMI160::CMD, BMI160::FOC_START); + + daplink.printf("Start..."); + orient1.init(); wait(1); gLED = 0; bLED = 0; - pc.printf("\033[H"); //home - pc.printf("\033[0J"); //erase from cursor to end of screen - uint32_t failures = 0; - if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) - { - pc.printf("Failed to set gyroscope power mode\r\n"); - failures++; - } - wait_ms(100); - - if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) - { - pc.printf("Failed to set accelerometer power mode\r\n"); - failures++; - } - wait_ms(100); - - - BMI160::AccConfig accConfig; - //example of using getSensorConfig - if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) - { - pc.printf("ACC Range = %d\r\n", accConfig.range); - pc.printf("ACC UnderSampling = %d\r\n", accConfig.us); - pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); - pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); - } - else - { - pc.printf("Failed to get accelerometer configuration\r\n"); - failures++; - } - - //example of setting user defined configuration - accConfig.range = BMI160::SENS_2G; //rage is 2g - accConfig.us = BMI160::ACC_US_OFF; //undersampling is off - accConfig.bwp = BMI160::ACC_BWP_0; //average 4 cycles - accConfig.odr = BMI160::ACC_ODR_9; //output data rate - if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) - { - pc.printf("ACC Range = %d\r\n", accConfig.range); - pc.printf("ACC UnderSampling = %d\r\n", accConfig.us); - pc.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp); - pc.printf("ACC OutputDataRate = %d\r\n\r\n", accConfig.odr); - } - else - { - pc.printf("Failed to set accelerometer configuration\r\n"); - failures++; - } - - BMI160::GyroConfig gyroConfig; - if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) + + while(1) { - pc.printf("GYRO Range = %d\r\n", gyroConfig.range); - pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp); - pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr); - - - } - else - { - pc.printf("Failed to get gyroscope configuration\r\n"); - failures++; - } - - wait(1.0); - pc.printf("\033[H"); //home - pc.printf("\033[0J"); //erase from cursor to end of screen - - if(failures == 0) - { - - - float imuTemperature; - - double xDeviation, yDeviation, zDeviation; - double prevGyroX, prevGyroY, prevGyroZ; - double currentGyroX, currentGyroY, currentGyroZ; - double diffGyroX, diffGyroY, diffGyroZ; - - double xDisplacement, yDisplacement, zDisplacement; - double currentAccX, currentAccY, currentAccZ; - double prevAccX, prevAccY, prevAccZ; - double diffAccX, diffAccY, diffAccZ; - - double xVelocity, yVelocity, zVelocity; - double timeDiff, time_1, time_2; - bool timeFlag = false; - BMI160::SensorData accData; - BMI160::SensorData gyroData; - BMI160::SensorTime sensorTime; - - //PwmPin = 1; - float apitch = 0; - float k = 0.6; - - time_1 = sensorTime.seconds; - while(1) - { - - imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); - imu.getTemperature(&imuTemperature); - - time_2 = sensorTime.seconds; - apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1); - daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch); - time_1 = time_2; - - //printRegister(imu, BMI160::GYR_CONF); - - } + orient1.updatePitch(); + daplink.printf("Forward: %s%4.3f\r\n", "\033[K", orient1.getPitch()); } } -/* - if(timeFlag == true){ - - currentGyroX = gyroData.xAxis.scaled; - currentAccX = accData.xAxis.scaled; - - - diffGyroX = abs(currentGyroX - prevGyroX); - diffAccX = abs(currentAccX - prevAccX); - - time_2 = sensorTime.seconds; - timeDiff = time_2 - time_1; - - if (diffGyroX > 2){ - xDeviation = xDeviation + (gyroData.xAxis.scaled * (timeDiff)); - } - if (diffAccX > 0.009){ - xDisplacement = (xVelocity * timeDiff) + (0.5 * accData.xAxis.scaled * timeDiff * timeDiff); - xVelocity = xVelocity + (accData.xAxis.scaled * timeDiff); - } - - // pc.printf("%s%4.3f\r\n", "\033[K", xDeviation); - - //control motor for proportional linearity - - if(xDeviation < 0.0) - { - M_1 = 0; - M_2 = 1; - daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation); - } - else{ - M_1 = 1; - M_2 = 0; - daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation); - } - - //------------------------ - - - //pc.printf("Velocity: %s%4.3f\r\n", "\033[K", prevAccX); - // pc.printf("Interval: %s%4.3f\r\n", "\033[K", timeDiff); - //pc.printf("%s%4.3f\r\n\r\n", "\033[K", xDisplacement); - - prevGyroX = currentGyroX; - prevAccX = currentAccX; - time_1 = time_2; - } - else{ - time_1 = sensorTime.seconds; - timeFlag = true; - } - - - gLED = !gLED; - wait_ms(1); - } - } - else - { - while(1) - { - rLED = !rLED; - wait(0.6); - } - } -*/ - - - - -//***************************************************************************** -void dumpImuRegisters(BMI160 &imu) -{ - printRegister(imu, BMI160::CHIP_ID); - printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA); - printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1); - printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST); - printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1); - printRegister(imu, BMI160::CMD); - pc.printf("\r\n"); -} - - -//***************************************************************************** -void printRegister(BMI160 &imu, BMI160::Registers reg) -{ - uint8_t data; - if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR) - { - pc.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); - daplink.printf("IMU Register 0x%02x = 0x%02x\r\n", reg, data); - } - else - { - pc.printf("Failed to read register\r\n"); - } -} - -//***************************************************************************** -void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data) -{ - imu.writeRegister(reg, data); - - -} - - -//***************************************************************************** -void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) -{ - uint8_t numBytes = ((stopReg - startReg) + 1); - uint8_t buff[32]; - uint8_t offset = static_cast<uint8_t>(startReg); - - if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR) - { - for(uint8_t idx = offset; idx < (numBytes + offset); idx++) - { - pc.printf("IMU Register 0x%02x = 0x%02x\r\n", idx, buff[idx - offset]); - } - } - else - { - pc.printf("Failed to read block\r\n"); - } -} - - -/* An example for configuring FOC for accel and gyro data */