Revision 2:f89067092cef, committed 2018-01-18

Comitter:

JohanBeverini

Date:

Thu Jan 18 13:50:34 2018 +0000

Parent:

1:8f6591373cfd

Commit message:

version apres noel

Changed in this revision

diff -r 8f6591373cfd -r f89067092cef main.cpp
--- a/main.cpp	Fri Nov 24 11:30:50 2017 +0000
+++ b/main.cpp	Thu Jan 18 13:50:34 2018 +0000
@@ -2,14 +2,14 @@
 #include "MPU6050.h"
 
 Serial PC(SERIAL_TX, SERIAL_RX);
-Serial BT(PA_10, PA_9);
-
+Serial BT(PA_9, PA_10);
 MPU6050 mpu6050;
 Ticker t;
+Timer t1;
 
 DigitalOut myled(LED1);
 
-float alpha, beta, gamma, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode;
+float alpha, betaa, gammaa, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode, sumCount, sum;
 
 void recup_MPU(void){
         
@@ -34,7 +34,7 @@
     temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
    }
     
-    Now = t.read_us();
+    Now = t1.read_us();
     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
     lastUpdate = Now;
     
@@ -49,6 +49,7 @@
    // Pass gyro rate as rad/s
     mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
     
+    (gx,gy,gz)=(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
     
             //sensorX <= accelCount[0];
             //sensorY <= accelCount[1];
@@ -58,34 +59,100 @@
             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("gyroscope in X = %u, or %f rad/s\n", (unsigned int)gyroCount[0], gx);
+            PC.printf("gyroscope in Y = %u, or %f rad/s\n", (unsigned int)gyroCount[1], gy);
+            PC.printf("gyroscope in Z = %u, or %f rad/s\n", (unsigned int)gyroCount[2], gz);
             
             PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
 }
 
+
+
+
+
 void boucle(void){
     recup_MPU();
     alpha+=gx*periode;
-    beta+=gy*periode;
-    gamma+=gz*periode;
+    betaa+=gy*periode;
+    gammaa+=gz*periode;
     
+    BT.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
+    
+    myled!=myled;
     
 }
 
+
+
+
+
 int main()
 {
     PC.baud(9600);  
     PC.printf("Hello World !\n");
-    BT.baud(9600);  
+    BT.baud(38400);  
     BT.printf("Connection BT\n");
     
-    periode=0.1;
+    periode=1;
     
     alpha=0.0;
-    beta=0.0;
-    gamma=0.0;        
+    betaa=0.0;
+    gammaa=0.0;        
+    
+
+
+
+
+    /////////////////////////////////////////////////////////////////////////////////////////////////
+  // Read the WHO_AM_I register, this is a good test of communication
+  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+  PC.printf("I AM 0x%x\n\r", whoami);
+   PC.printf("I SHOULD BE 0x68\n\r");
+  
+  if (whoami == 0x68) // WHO_AM_I should always be 0x68
+  {  
+    PC.printf("MPU6050 is online...");
+    wait(1);
+    
+    mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+    PC.printf("x-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[0]); PC.printf("% of factory value \n\r");
+    PC.printf("y-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[1]); PC.printf("% of factory value \n\r");
+    PC.printf("z-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[2]); PC.printf("% of factory value \n\r");
+    PC.printf("x-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[3]); PC.printf("% of factory value \n\r");
+    PC.printf("y-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[4]); PC.printf("% of factory value \n\r");
+    PC.printf("z-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[5]); PC.printf("% of factory value \n\r");
+    wait(1);
+
+    if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
+    {
+    mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+    mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+    mpu6050.initMPU6050(); PC.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+
+    wait(2);
+       }
+    else
+    {
+    PC.printf("Device did not the pass self-test!\n\r");
+     
+      }
+    }
+    else
+    {
+    PC.printf("Could not connect to MPU6050: \n\r");
+    PC.printf("%#x \n",  whoami);
+    
+    while(1) ; // Loop forever if communication doesn't happen
+    }
+    
+    PC.printf("init sensor done\n");
+    /////////////////////////////////////////////////////////////////////////////////////
+    
+    
+    
+    
+  
+    t1.start();  
     
     recup_MPU();
     poidx=ax;
@@ -97,8 +164,14 @@
     
     
     
+    
     while(1) {
 
+        char c = BT.getc();
+        
+        if(c == 'a') {
+            BT.printf("\nOK\n");
+        }
     
     }
 }