Diff: main.cpp

Revision:

0:1d41bd249237

Child:

1:8f6591373cfd

diff -r 000000000000 -r 1d41bd249237 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 24 11:10:42 2017 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "MPU6050.h"
+
+Serial PC(SERIAL_TX, SERIAL_RX);
+Serial BT(PA_10, PA_9);
+
+MPU6050 mpu6050;
+
+DigitalOut myled(LED1);
+
+int main()
+{
+    PC.baud(9600);  
+    PC.printf("Hello World !\n");
+    BT.baud(9600);  
+    BT.printf("Connection BT\n");
+    
+    
+    
+    
+    while(1) {
+        
+        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+    mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+    mpu6050.getAres();
+    
+    // Now we'll calculate the accleration value into actual g's
+    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+    ay = (float)accelCount[1]*aRes - accelBias[1];   
+    az = (float)accelCount[2]*aRes - accelBias[2];  
+   
+    mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+    mpu6050.getGres();
+ 
+    // Calculate the gyro value into actual degrees per second
+    gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+    gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
+    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
+
+    tempCount = mpu6050.readTempData();  // Read the adc values
+    temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+   }
+    
+    Now = t.read_us();
+    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+    lastUpdate = Now;
+    
+    sum += deltat;
+    sumCount++;
+    
+    if(lastUpdate - firstUpdate > 10000000.0f) {
+     beta = 0.04;  // decrease filter gain after stabilized
+     zeta = 0.015; // increasey bias drift gain after stabilized
+    }
+    
+   // Pass gyro rate as rad/s
+    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+    
+    
+            //sensorX <= accelCount[0];
+            //sensorY <= accelCount[1];
+            //sensorZ <= accelCount[2];
+            
+            PC.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
+            PC.printf("acceleration in Y = %u, or %f g\n", (unsigned int)accelCount[1], ay);
+            PC.printf("acceleration in Z = %u, or %f g\n", (unsigned int)accelCount[2], az);
+            
+            PC.printf("gyroscope in X = %u, or %f dps\n", (unsigned int)gyroCount[0], gx);
+            PC.printf("gyroscope in Y = %u, or %f dps\n", (unsigned int)gyroCount[1], gy);
+            PC.printf("gyroscope in Z = %u, or %f dps\n", (unsigned int)gyroCount[2], gz);
+            
+            PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
+    }
+}