TSDA Robotics
SelfBalancing
Dependencies: mbed BMI160 max32630fthr USBDevice Math MAX14690
Revision 4:fc8ef84a7dbc, committed 2018-12-05
- Comitter:
- CharlesMaxim
- Date:
- Wed Dec 05 04:44:17 2018 +0000
- Parent:
- 3:ad1e61509a89
- Commit message:
- ...
Changed in this revision
diff -r ad1e61509a89 -r fc8ef84a7dbc BMI160.lib --- a/BMI160.lib Thu Aug 16 14:34:37 2018 +0300 +++ b/BMI160.lib Wed Dec 05 04:44:17 2018 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/MaximIntegrated/code/BMI160/#a521606048bb +https://os.mbed.com/teams/TSDA-Robotics/code/BMI160/#0b28b9d13164
diff -r ad1e61509a89 -r fc8ef84a7dbc MAX14690.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MAX14690.lib Wed Dec 05 04:44:17 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/MaximIntegrated/code/MAX14690/#264f38840873
diff -r ad1e61509a89 -r fc8ef84a7dbc Math.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Math.lib Wed Dec 05 04:44:17 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/inst/code/Math/#067c036b09e0
diff -r ad1e61509a89 -r fc8ef84a7dbc PinNames.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PinNames.h Wed Dec 05 04:44:17 2018 +0000 @@ -0,0 +1,8 @@ +#ifndef PinNames_h +#define PinNames_h + +#define outputPort P3_5 +#define pulseWidthPinControl AIN_3 +#define periodPinControl AIN_2 + +#endif \ No newline at end of file
diff -r ad1e61509a89 -r fc8ef84a7dbc USBDevice.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice.lib Wed Dec 05 04:44:17 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/MaximIntegrated/code/USBDevice/#dad310740b28
diff -r ad1e61509a89 -r fc8ef84a7dbc main.cpp
--- 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 */
diff -r ad1e61509a89 -r fc8ef84a7dbc max32630fthr.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/max32630fthr.lib Wed Dec 05 04:44:17 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/CharlesMaxim/code/max32630fthr/#346783d1f8e1
diff -r ad1e61509a89 -r fc8ef84a7dbc max32630hsp.lib --- a/max32630hsp.lib Thu Aug 16 14:34:37 2018 +0300 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/teams/MaximIntegrated/code/max32630hsp2/#60c0cc971d85 \ No newline at end of file