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 */