SelfBalancing
Dependencies: mbed BMI160 max32630fthr USBDevice Math MAX14690
Diff: main.cpp
- Revision:
- 4:fc8ef84a7dbc
- Parent:
- 2:ba0a55e05168
--- a/main.cpp Thu Aug 16 14:34:37 2018 +0300 +++ b/main.cpp Wed Dec 05 04:44:17 2018 +0000 @@ -33,27 +33,55 @@ #include "mbed.h" #include "bmi160.h" -#include "max32630hsp.h" +//#include "max32630hsp.h" +#include "max32630fthr.h" +#include "USBSerial.h" +#include "stdlib.h" -MAX32630HSP icarus(MAX32630HSP::VIO_3V3); +//MAX32630HSP icarus(MAX32630HSP::VIO_3V3); +MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); + +Serial daplink(P2_1, P2_0); -DigitalOut rLED(LED1, LED_OFF); -DigitalOut gLED(LED2, LED_OFF); -DigitalOut bLED(LED3, LED_OFF); +DigitalOut rLED(P2_4, LED_ON); +DigitalOut gLED(P2_5, LED_ON); +DigitalOut bLED(P2_6, LED_ON); -Serial pc(USBTX,USBRX); +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); 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); + + wait(1); + gLED = 0; + bLED = 0; pc.printf("\033[H"); //home pc.printf("\033[0J"); //erase from cursor to end of screen @@ -91,10 +119,10 @@ } //example of setting user defined configuration - accConfig.range = BMI160::SENS_4G; - accConfig.us = BMI160::ACC_US_OFF; - accConfig.bwp = BMI160::ACC_BWP_2; - accConfig.odr = BMI160::ACC_ODR_8; + 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); @@ -127,30 +155,92 @@ 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; while(1) { + imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range); imu.getTemperature(&imuTemperature); - - pc.printf("ACC xAxis = %s%4.3f\r\n", "\033[K", accData.xAxis.scaled); - pc.printf("ACC yAxis = %s%4.3f\r\n", "\033[K", accData.yAxis.scaled); - pc.printf("ACC zAxis = %s%4.3f\r\n\r\n", "\033[K", accData.zAxis.scaled); + + //printRegister(imu, BMI160::GYR_CONF); + + if(timeFlag == true){ + + currentGyroX = gyroData.xAxis.scaled; + currentAccX = accData.xAxis.scaled; - pc.printf("GYRO xAxis = %s%5.1f\r\n", "\033[K", gyroData.xAxis.scaled); - pc.printf("GYRO yAxis = %s%5.1f\r\n", "\033[K", gyroData.yAxis.scaled); - pc.printf("GYRO zAxis = %s%5.1f\r\n\r\n", "\033[K", gyroData.zAxis.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; + pc.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation); + } + else{ + M_1 = 1; + M_2 = 0; + pc.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); - pc.printf("Sensor Time = %s%f\r\n", "\033[K", sensorTime.seconds); - pc.printf("Sensor Temperature = %s%5.3f\r\n", "\033[K", imuTemperature); - - pc.printf("\033[H"); //home + prevGyroX = currentGyroX; + prevAccX = currentAccX; + time_1 = time_2; + } + else{ + time_1 = sensorTime.seconds; + timeFlag = true; + } + + gLED = !gLED; - wait(0.5); + wait_ms(1); } } else @@ -158,7 +248,7 @@ while(1) { rLED = !rLED; - wait(0.25); + wait(0.6); } } } @@ -184,6 +274,7 @@ 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 { @@ -191,6 +282,14 @@ } } +//***************************************************************************** +void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data) +{ + imu.writeRegister(reg, data); + + +} + //***************************************************************************** void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg) @@ -211,3 +310,6 @@ pc.printf("Failed to read block\r\n"); } } + + +/* An example for configuring FOC for accel and gyro data */