Diff: main.cpp

Revision:

9:6c803986dbde

Parent:

8:9e6ead2ee8d7

Child:

10:f5ba762b58b4

--- a/main.cpp	Tue Aug 21 13:25:37 2018 +0000
+++ b/main.cpp	Mon Sep 03 10:39:56 2018 +0000
@@ -2,18 +2,17 @@
 #include <math.h>
 #include <inttypes.h>
 #include "ADXL355.h"
-#include "ADXRS290.h"
 #include "CALIBRATION.h"
  
 Serial pc(USBTX, USBRX);
  
 ADXL355 accl(SPI1_CS0, SPI1_MOSI, SPI1_MISO, SPI1_SCLK);    // PMOD port
-ADXRS290 gyro(SPI0_CS2, SPI0_MOSI, SPI0_MISO, SPI0_SCLK);   // PMOD port
 CALIBRATION test;
+#define pi 3.14159265;
 float angle[3][12] = {  {-55, -125, -147, 33, -128, 52, 0, 0, 0, 0, 0, 0},
                         {6, -6, 20, -20, -69, 69, 0, 0, 0, 0, 0, 0 }, 
                         {1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}}; 
-    float meas[3][12] =  {  {-0.09, 0.16,  -0.31,  0.37,  0.91,  -0.88},
+float meas[3][12] =  {  {-0.09, 0.16,  -0.31,  0.37,  0.91,  -0.88},
                             {-0.78, 0.81,  -0.46,  0.49,  -0.28,  0.32},
                             {0.54,  -0.55, -0.78,  0.76,  -0.20,  0.20}};   
 int main(){
@@ -34,32 +33,137 @@
     accl.set_power_ctl_reg(accl.MEASUREMENT);
     d=accl.read_reg(accl.POWER_CTL);
     pc.printf("power control on measurement mode = %x \r\n",d);
-    float x, y,z;
+    float x, y, z, w, k, h;
     float t;
     // save data info a file
+    pc.printf("x \t y \t z \t t\r\n");
     for(int i=0; i<100; i++) {
         x = accl.convert(accl.scanx())*accl.axis355_sens;
         y = accl.convert(accl.scany())*accl.axis355_sens;
         z = accl.convert(accl.scanz())*accl.axis355_sens;
         t = 25+float(accl.scant()-1852)/(-9.05);
-        pc.printf("%f \t %f \t %f  \t %f \r\n" , x,y,z,t);
-        wait(0.1);}    // test the calibration procedure
-   // test.matrix_reset();
-   // test.matrix_g(angle);
-   // test.matrix_x(meas);
-   // int i, j;
- 
-   /* output each array element's value */
-   /*
-    pc.printf("power control on measurement mode = %x \r\n",d);
-   for ( i = 0; i < 3; i++ ) {
-         for ( j = 0; j < 12; j++ ) {
-             pc.printf( "%f\n", i,j, test.g_matrix[i][j] );
-      }
-    pc.printf("\r\n");
-     */ 
-      
-   //}
-    
+        pc.printf("%f \t %f \t %f  \t %f   \r\n" , x,y,z,t);
+        wait(0.1);}    
+   // test the calibration procedure
+   test.matrix_reset();
+   pc.printf("Start calibration test (y/n)?\r\n");
+   char calib = pc.getc();
+   switch(calib){
+       case 'y': pc.printf("Select the calibration procedure: 2point or 4point? (2/4)\r\n");
+                char point = pc.getc();
+                if(point == '2'){
+                    pc.printf("Place the device verticaly to obtain x value close to -1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; y = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x<y) {y=x;}
+                        pc.printf("%f  \t %f \r\n" , x,y);
+                        wait(0.1);}
+                    pc.printf("Place the device verticaly to obtain x value close to +1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; z = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x>z) {z=x;}
+                        pc.printf("%f  \t %f \r\n" , x,z);
+                        wait(0.1);}     
+                    float meas[] = {z, y};
+                    test.convert_2p(meas, 0);
+                    pc.printf("The input for the calibration process are: %f  \t %f \r\n" , meas[0], meas[1]);
+                    pc.printf("The result (Sensibility and Bias) is: %f  \t %f \r\n" , test.adxl355_sensitivity.S[0], test.adxl355_sensitivity.B[0]);
+                    }
+                else if(point == '4'){
+                    pc.printf("Place the device verticaly to obtain x value close to 0, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; y = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        y=(y+x)/2;
+                        pc.printf("%f  \t %f \r\n" , x,y);
+                        wait(0.1);}
+                    pc.printf("Rotate the device to obtain x value close to -1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; z = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x<z) {z=x;}
+                        pc.printf("%f  \t %f \r\n" , x,z);
+                        wait(0.1);}
+                    pc.printf("Place the device verticaly to obtain x value close to 0, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; w = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        w=(w+x)/2;
+                        pc.printf("%f  \t %f \r\n" , x,w);
+                        wait(0.1);}
+                    pc.printf("Rotate the device verticaly to obtain x value close to +1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; k = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x>k) {k=x;}
+                        pc.printf("%f  \t %f \r\n" , x,k);
+                        wait(0.1);}     
+                    float meas[] = {y, z, w, k};
+                    test.convert_4p(meas, 0);
+                    pc.printf("The input for the calibration process are: %f  \t %f \t %f \t %f \t \r\n" , y, z, w, k);
+                    pc.printf("The result (Sensibility and Bias) is: %f  \t %f \r\n" , test.adxl355_sensitivity.S[0], test.adxl355_sensitivity.B[0]);
+                    }
+                else {pc.printf("Input value not recognized \r\n"); pc.printf("%c \r\n",point);}
+                break;
+       case 'n': pc.printf("No calibration test \r\n"); 
+                break;
+       default:  pc.printf("Input value not recognized \r\n"); 
+                break;
+       }     
+    // test reading angle
+   pc.printf("Start reading angle after calibration (y/n)?\r\n");
+   calib = pc.getc();
+   switch(calib){
+       case 'y': pc.printf("Select the procedure for angle measurement: 1axis, 2axis or 3axis? (1/2/3)\r\n");
+                char point = pc.getc();
+                pc.printf("x \t y \t z \t pitch \t roll \t tau \r\n");
+                if(point == '1'){
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        y = accl.convert(accl.scany())*accl.axis355_sens;
+                        z = accl.convert(accl.scanz())*accl.axis355_sens;
+                        w = asin(x/9.81)*180/pi;
+                        k = asin(y/9.81)*180/pi;
+                        h = asin(z/9.81)*180/pi;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);}   
+                    }
+                else if(point == '2'){
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        y = accl.convert(accl.scany())*accl.axis355_sens;
+                        z = accl.convert(accl.scanz())*accl.axis355_sens;
+                        w = atan(x/z)*180/pi;;
+                        k = atan(y/z)*180/pi;;
+                        h = 0;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);}
+                    }
+                else if(point == '3'){
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        y = accl.convert(accl.scany())*accl.axis355_sens;
+                        z = accl.convert(accl.scanz())*accl.axis355_sens;
+                        w = atan2(x,sqrt(pow(y,2)+pow(z,2)))*180/pi; 
+                        k = atan2(y,sqrt(pow(x,2)+pow(z,2)))*180/pi;
+                        h = atan2(sqrt(pow(x,2)+pow(y,2)),z)*180/pi;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);} 
+                    }  
+                else {pc.printf("Input value not recognized \r\n"); pc.printf("%c \r\n",point);}
+                break;
+       case 'n': pc.printf("No calibration test \r\n"); 
+                break;
+       default:  pc.printf("Input value not recognized \r\n"); 
+                break;
+       }  
 }
  
\ No newline at end of file