Diff: main.cpp

Revision:

4:ee0a25a1c3dc

Parent:

3:7cef8e5abfe5

--- a/main.cpp	Sun Sep 04 08:04:35 2016 +0000
+++ b/main.cpp	Wed Apr 11 19:28:43 2018 +0000
@@ -1,28 +1,210 @@
-/****************************************************************************
- ****** 3-axis Accelerometer ADXL335 or GY-61 interfacing to FRDM-KL25Z *****
- ******                  Author :  SHIVAM TRIPATHI                     ******  
- ***************************************************************************/
+/*****************************************
+ ****** 3-axis Accelerometer ADXL335 *****  
+ *****************************************/
 
 #include "mbed.h"
 
-Serial pc(USBTX, USBRX);
-AnalogIn analog_value1(A0);  //Output of X-axis at analog pin A0  ........ (Refer pinout)
-AnalogIn analog_value2(A1);  //Output of y-axis at analog pin A1
-AnalogIn analog_value3(A2);  //Output of z-axis at analog pin A1
+#define ETALONNAGE 100
+#define VERIFICATION 100
+#define MESURE 500
+//#define LIMITE 65535 * 1 / 1000
+#define LIMITE 4095 * 1 / 1000
+#define LSB 14.365
+
+Timer timer;
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+AnalogIn acc_x(A0);     // Output of X-axis
+AnalogIn acc_y(A1);     // Output of y-axis
+AnalogIn acc_z(A2);     // Output of z-axis
+DigitalOut led(LED1);   // output of LED
+
+void mesure_zero_acceleration(int *zero_x, int *zero_y, int *zero_z)
+{
+    float fx, fy, fz;
+    float f_etalonnage = (float)ETALONNAGE;
+    int x, y, z;
+    int c, k;
+    
+    for (c = 0; c < ETALONNAGE; c++)
+    {
+           x = 0;
+           y = 0;
+           z = 0;
+           for (k = 0; k < MESURE; k++)
+           {
+              x += acc_x.read_u16() >> 4;
+              y += acc_y.read_u16() >> 4;
+              z += acc_z.read_u16() >> 4;
+           }
+           *zero_x += x;
+           *zero_y += y;
+           *zero_z += z;
+    }
+    fx = *zero_x / f_etalonnage;
+    fy = *zero_y / f_etalonnage;
+    fz = *zero_z / f_etalonnage;
+    *zero_x /= ETALONNAGE;
+    *zero_y /= ETALONNAGE;
+    *zero_z /= ETALONNAGE;
+    
+    if ((fx - *zero_x) >= 0.5) *zero_x ++;
+    if ((fy - *zero_y) >= 0.5) *zero_y ++;
+    if ((fz - *zero_z) >= 0.5) *zero_z ++;
+    
+    pc.printf("AX_0;AY_0;AZ_0\n");
+    pc.printf("%d;%d;%d\n", *zero_x, *zero_y, *zero_z);
+}
 
-DigitalOut led(LED1);
+void verif_zero_acceleration(int zero_x, int zero_y, int zero_z)
+{
+    int x, y, z;
+    float fx, fy, fz;
+    float f_mesure = (float)MESURE;
+    int c, k;
+    
+    for (c = 0; c < VERIFICATION; c++)
+    {
+        x = 0; y = 0; z = 0;
+        
+        for (k = 0; k < MESURE; k++)
+        {
+            x += ( acc_x.read_u16() >> 4 );
+            y += ( acc_y.read_u16() >> 4 );
+            z += ( acc_z.read_u16() >> 4 );
+        }
+        
+        x -= zero_x;
+        y -= zero_y;
+        z -= zero_z;
+        
+        fx = x / f_mesure;
+        fy = y / f_mesure;
+        fz = z / f_mesure;
+        x /= MESURE;
+        y /= MESURE;
+        z /= MESURE;
+        
+        if (fx < 0)
+        {
+            if ( (fx - x) <= - 0.5 ) x--;
+        }else
+            if ( (fx - x) >= 0.5 ) x++;
+        if (fy < 0)
+        {
+            if ( (fy - y) <= - 0.5 ) y--;
+        }else
+            if ( (fy - y) >= 0.5 ) y++;
+        if (fz < 0)
+        {
+            if ( (fz - z) <= - 0.5 ) z--;
+        }else
+            if ( (fz - z) >= 0.5 ) z++;
+        
+        if( abs (x) > LIMITE )
+        {
+             pc.printf("Etalonnage en X manque! Ecart = %d\n", abs(x) - LIMITE);
+             exit(EXIT_FAILURE);
+        }
+        if( abs (y) > LIMITE > LIMITE )
+        {
+             pc.printf("Etalonnage en Y manque! Ecart = %d\n", abs(y) - LIMITE);
+             exit(EXIT_FAILURE);
+        }
+        if( abs (z) > LIMITE )
+        {
+            pc.printf("Etalonnage en Z manque! Ecart = %d\n", abs(z) - LIMITE);
+            exit(EXIT_FAILURE);
+        }
+    }
+}
 
 int main() {
-    int x,y,z;
+    int ax, ay, az;                            // Store the acceleration
+    /*
+    float vx = 0, vy = 0, vz = 0;              // Store the speed
+    float x = 0, y = 0, z = 0;                 // Store the position
+    */
+    float fx, fy, fz;
+    float f_mesure = (float)MESURE;
+    int zero_x = 0, zero_y = 0, zero_z = 0; // 0 acceleration
+    float t;                                // Measurement Time
+    int i, cpt = 0;                         // Counters
     
-    printf("\nAnalogIn example\n");
+    wait(2);
+    
+    timer.start();
+    
+    pc.baud(9600);
+    
+    mesure_zero_acceleration(&zero_x, &zero_y, &zero_z);
+    //verif_zero_acceleration(zero_x, zero_y, zero_z);
+    
+    led = 1;
+    
+    pc.printf("Temps d'acquisition;AX;AY;AZ;Numero d'echantillon\n");
+    //pc.printf("AX;AY;AZ;VX;VY;VZ;X;Y;Z\n");
     
     while(1) {
-        x = analog_value1.read_u16();           // Reads X-axis value and then converts in 16 bit format (3.3V --> 65535).........Analog values are read
-        y = analog_value2.read_u16();           // Reads Y-axis value and then converts in 16 bit format (3.3V --> 65535)
-        z = analog_value3.read_u16();           // Reads Z-axis value and then converts in 16 bit format (3.3V --> 65535)
-        printf("\r x = %d  y = %d  z = %d  \n", x,y,z);    //Prints output on pc Serial terminal (serial USB com driver is a must)
-        wait(0.5); // 500 ms
+        cpt ++;
+        ax = 0; ay = 0; az = 0;
+        //timer.start();
+        for (i = 0; i < MESURE; i++)
+        {
+            ax += ( acc_x.read_u16() >> 4 );           // Reads X-axis value (on 16 bits converted to 12)
+            ay += ( acc_y.read_u16() >> 4 );           // Reads Y-axis value
+            az += ( acc_z.read_u16() >> 4 );           // Reads Z-axis value
+        }
+        //timer.stop();
+        t = timer.read();
+        //timer.reset();
+        
+        ax -= zero_x;
+        ay -= zero_y;
+        az -= zero_z;
+    
+        fx = ax / f_mesure;
+        fy = ay / f_mesure;
+        fz = az / f_mesure;
+        ax /= MESURE;
+        ay /= MESURE;
+        az /= MESURE;
         
+        if (fx < 0)
+        {
+            if ( (fx - ax) <= - 0.5 ) ax--;
+        }else
+            if ( (fx - ax) >= 0.5 ) ax++;
+        if (fy < 0)
+        {
+            if ( (fy - ay) <= - 0.5 ) ay--;
+        }else
+            if ( (fy - ay) >= 0.5 ) ay++;
+        if (fz < 0)
+        {
+            if ( (fz - az) <= - 0.5 ) az--;
+        }else
+            if ( (fz - az) >= 0.5 ) az++;
+    
+        if(ax == -1 || ax == 1) ax = 0;
+        if(ay == -1 || ay == 1) ay = 0;
+        if(az == -1 || az == 1) az = 0;
+    
+        pc.printf("%f;%d;%d;%d;%d\n", t, ax, ay, az, cpt);
+        
+        /*
+        
+        vx += (ax * t * LSB);
+        vy += (ay * t * LSB);
+        vz += (az * t * LSB);
+        
+        x += (vx * t * LSB);
+        y += (vy * t * LSB);
+        z += (vz * t * LSB);
+        
+        if (cpt % 16 == 0) pc.printf("%d;%d;%d;%d;%d;%d;%d;%d;%d\n", ax, ay, az, vx, vy, vz, x, y, z);   
+        
+        */    
     }
-}
+}
\ No newline at end of file