Diff: main.cpp

Revision:

3:3ee58f31a637

Parent:

2:89f008ca5911

--- a/main.cpp	Thu Dec 07 10:38:26 2017 +0000
+++ b/main.cpp	Sat Dec 09 05:48:49 2017 +0000
@@ -3,7 +3,7 @@
 #include "ble/services/UARTService.h"
 #include "Serial.h"
 #include "ADXL345.h"
-#include "mbed.h"
+
 
 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
                                * it will have an impact on code-size and power consumption. */
@@ -55,35 +55,71 @@
     
 }
 
+int threshold =80;
+int xval[10]={0};
+int yval[10]={0};
+int zval[10]={0};
+int xavg;
+int yavg;
+int zavg;
+
+
+int calibratex()
+{
+   int readings[3] = {0, 0, 0};
+   led1=1;
+   long int sum1  = 0;
+   for (int i=0; i<10; i++)
+   {
+       accelerometer.getOutput(readings);
+       xval[i]=(readings[0]);  //nilai x adxl345
+       sum1 = yval[i]+sum1;
+   }
+   
+   xavg=sum1/10.0;
+
+   led1=0;     
+   return xavg;
+}
+int calibratey()
+{
+   int readings[3] = {0, 0, 0};
+   led1=1;
+   long int sum2 = 0;
+   for (int i=0; i<10; i++)
+   {
+       accelerometer.getOutput(readings);
+        yval[i]=(readings[1]); //nilai y adxl345
+       sum2 = yval[i]+sum2;
+   }
+   yavg=sum2/10.0;
+   led1=0;     
+   return yavg;
+}
+int calibratez()
+{
+   int readings[3] = {0, 0, 0};
+    
+   led1=1;
+   long int sum3 = 0;
+   for (int i=0; i<10; i++)
+   {
+       accelerometer.getOutput(readings);
+        zval[i]=(readings[2]); //nilai z adxl345
+       sum3 = zval[i]+sum3;
+   }
+   
+   zavg=sum3/10.0;
+   led1=0;     
+   return zavg;
+}
+
 
 int main() 
 {
     uart1.baud(9600);
     int readings[3] = {0, 0, 0};
-    uart1.printf("Starting ADXL345 test...\n");
-    uart1.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
-
-    //Go into standby mode to configure the device.
-    accelerometer.setPowerControl(0x00);
-
-    //Full resolution, +/-16g, 4mg/LSB.
-    accelerometer.setDataFormatControl(0x0B);
-    
-    //3.2kHz data rate.
-    accelerometer.setDataRate(ADXL345_3200HZ);
-
-    //Measurement mode.
-    accelerometer.setPowerControl(0x08);
-    
     char buffer [20];
-       
-    // Test Daata
-    memset(&buffer, 0, sizeof(buffer));
-    int16_t reading_1 = 54;
-    int16_t reading_2 = 42;
-    int16_t reading_3 = 32;
-       
-    snprintf(buffer,20,"data: %d, %d, %d\n", reading_1, reading_2, reading_3);
     
     led1 = 1;
     uart1.baud(9600);
@@ -109,19 +145,135 @@
 
     UARTService uartService(ble);
     uartServicePtr = &uartService;
+    
+    uart1.printf("Starting ADXL345 test...\n");
+    uart1.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
 
-    while (1) {
-        ble.waitForEvent();
-        wait(0.1);
-        accelerometer.getOutput(readings);
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+    
+    //3.2kHz data rate.
+    accelerometer.setDataRate(ADXL345_3200HZ);
+
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+    
+    int avg1 = calibratex();
+    int avg2 = calibratey();
+    int avg3 = calibratez();
+       
+        //inisiliasi
+        int steps=0;
+        int flag=0;
+        int acc=0;
+        int totvect   [20] = {0};
+        int totave    [20] = {0};
+        int totvel    [20] = {0};
+        int totdist   [20] = {0};
+        int totheight [20] = {0};
+            
         
-        //13-bit, sign extended values.
-        uart1.printf("%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
-        
-        memset(&buffer, 0, sizeof(buffer));
-        snprintf(buffer, 20, "data: %d, %d, %d\n", (int16_t)readings[0],(int16_t)readings[1], (int16_t)readings[2]);
-        ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+        //float sum1, sum2, sum3 = 0
+        int xaccl[20];
+        int yaccl[20];
+        int zaccl[20];
+       
+       // Test Daata
+       memset(&buffer, 0, sizeof(buffer));
+/*       int16_t reading_1= 0;
+       int16_t reading_2= 0;
+       int16_t reading_3= 0;
+       int16_t avg_1= 0;
+       int16_t avg_2= 0;
+       int16_t avg_3= 0;*/
+       
+       int16_t lang     = 0;
+       int16_t perc     = 0;
+       int16_t kec      = 0;
+       int16_t jar      = 0;
+       int16_t ting     = 0;
+       snprintf(buffer, 20, "data: %d,%d,%d,%d,%d\n",(int16_t)lang,(int16_t)perc,(int16_t)kec,(int16_t)jar,(int16_t)ting);
+/*       snprintf(buffer, 20, "data: %d,%d,%d,%d,%d,%d\n",(int16_t)reading_1,(int16_t)reading_2,(int16_t)reading_3,(int16_t)avg_1,(int16_t)avg_2,(int16_t)avg_3);*/
 
+    while (1) 
+    {
+        ble.waitForEvent();
+         wait(0.1);
+         accelerometer.getOutput(readings);
+         uart1.printf("\n%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
+/*         memset(&buffer, 0, sizeof(buffer));
+         snprintf(buffer, 20, "data: %d,%d,%d\n\n", (int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[0]);
+         ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);*/
+         
+         uart1.printf("\n%i, %i, %i\n", (int16_t)avg1, (int16_t)avg2, (int16_t)avg3);
+/*         memset(&buffer, 0, sizeof(buffer));
+         snprintf(buffer, 20, "data: %d,%d,%d\n\n", (int16_t)avg1,(int16_t)avg2,(int16_t)avg3);
+         ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);*/
+         wait(0.1);
+         
+         
+         
+         //float x,y,z
+        for (int i=0; i<10; i++)
+        {
+            xaccl[i]=(readings[0]);
+            wait(0.1);
+            yaccl[i]=(readings[1]);
+            wait(0.1);
+            zaccl[i]=(readings[2]);
+            wait(0.1);
+            
+            //formula
+            totvect[i] = sqrt((double)((xaccl[i]-xavg)* (xaccl[i]-xavg))+(double) ((yaccl[i] - yavg)*(yaccl[i] - yavg)) +(double) ((zaccl[i] - zavg)*(zaccl[i] - zavg)));
+            totave[i] = (totvect[i] + totvect[i-1]) / 2 ;
+            uart1.printf("\t acc: %i\n", (int16_t)totave[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "\t acc: %d\n", (int16_t)totave[i]);
+            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+
+            totvel[i]=(totave[i]*0.2);
+            uart1.printf("\t vel: %i\n", (int16_t)totvel[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "\t vel: %d\n", (int16_t)totvel[i]);
+            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+
+            totdist[i]=(totvect[i]*(0.2*0.2)/2);
+            uart1.printf("\t dist: %i\n", (int16_t)totdist[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "\tdist: %d\n", (int16_t)totdist[i]);
+            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+  
+            totheight[i]=sqrt((((xaccl[i]-xavg)* (xaccl[i]-xavg))+ ((yaccl[i] - yavg)*(yaccl[i] - yavg)))*(0.2*0.2)/2);
+            uart1.printf("\thght: %i\n", (int16_t)totheight[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "\thght: %d\n", (int16_t)totheight[i]);
+            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+  
+            acc=acc+totave[i];
+         
+         
+         
+         //cal steps
+         if (totave[i] > threshold && flag==0)
+         {
+             steps = steps+1;
+             flag=1;
+         }
+         else if (totave[i] > threshold && flag == 1)
+         {
+          // do nothing   
+         }
+         if (totave[i] < threshold && flag == 1)
+             {flag=0;}
+             uart1.printf("steps: %i\n", (int16_t)steps);
+             memset(&buffer, 0, sizeof(buffer));
+             snprintf(buffer, 20, "\nsteps: %d\t", (int16_t)steps);
+             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+             
+        }
     }
 
 }