Diff: main.cpp

Revision:

10:e5ee9515f4a2

Parent:

9:1e04b80d7199

Child:

11:a60e398f9032

--- a/main.cpp	Thu Dec 07 10:21:05 2017 +0000
+++ b/main.cpp	Sat Dec 09 02:06:19 2017 +0000
@@ -51,7 +51,7 @@
 {
     led1 = !led1;
 }
-/*
+
 int threshold =80;
 int xval[100]={0};
 int yval[100]={0};
@@ -59,156 +59,95 @@
 int xavg;
 int yavg;
 int zavg;
-int steps, flag=0;
-void calibrate(void)
+
+
+int calibratex()
 {
+   int readings[3] = {0, 0, 0};
    led1=1;
-   int sum1  = 0;
-   int sum2 = 0;
-   int sum3 = 0;
+   long int sum1  = 0;
    for (int i=0; i<100; i++)
    {
-       xval[i]=(1);  //nilai x adxl345
+       accelerometer.getOutput(readings);
+       xval[i]=(readings[0]);  //nilai x adxl345
        sum1 = xval[i]+sum1;
    }
-   wait(0.1);
+   
    xavg=sum1/100.0;
-   uart1.printf("%i\n", (int16_t)xavg); //nilai hasil kalibrasi x
-   
-   
+
+   led1=0;     
+   return xavg;
+}
+int calibratey()
+{
+   int readings[3] = {0, 0, 0};
+   led1=1;
+   long int sum2 = 0;
    for (int i=0; i<100; i++)
    {
-       yval[i]=(1); //nilai y adxl345
+       accelerometer.getOutput(readings);
+        yval[i]=(readings[1]); //nilai y adxl345
        sum2 = yval[i]+sum2;
    }
-   wait(0.1);
    yavg=sum2/100.0;
-   uart1.printf("%i\n", (int16_t)yavg); //nilai hasil kalibrasi y
-   
+   led1=0;     
+   return yavg;
+}
+int calibratez()
+{
+   int readings[3] = {0, 0, 0};
+    
+   led1=1;
+   long int sum3 = 0;
    for (int i=0; i<100; i++)
    {
-       zval[i]=(1); //nilai z adxl345
+       accelerometer.getOutput(readings);
+        zval[i]=(readings[2]); //nilai z adxl345
        sum3 = zval[i]+sum3;
    }
-   wait(0.1);
+   
    zavg=sum3/100.0;
-   uart1.printf("%i\n", (int16_t)zavg); //nilai hasil kalibrasi z
    led1=0;     
+   return zavg;
 }
-*/
+
+
 
  int main() 
  {
      uart1.baud(9600);
      int readings[3] = {0, 0, 0};
-     
-     //calibrate();
-     uart1.printf("Starting ADXL345 test...\n");
-     wait(.001);
-     uart1.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
-     wait(.001);
+     char buffer [20];
      
-    
-    //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);
-    
-    /*
+        //inisiliasi
+        int steps=0;
+        int flag=0;
         int acc=0;
-        int totvect   [100] = {0};
-        int totave    [100] = {0};
-        int totvel    [100] = {0};
-        int totdist   [100] = {0};
-        int totheight [100] = {0};
-        
+        int totvect   [20] = {0};
+        int totave    [20] = {0};
+        int totvel    [20] = {0};
+        int totdist   [20] = {0};
+        int totheight [20] = {0};
+            
         
         //float sum1, sum2, sum3 = 0
-        double xaccl[100] = {0};
-        double yaccl[100] = {0};
-        double zaccl[100] = {0};
-        
-        //float x,y,z
-        for (int i=0; i<100; i++)
-        {
-            xaccl[i]=(1);
-            wait(0.1);
-            yaccl[i]=(1);
-            wait(0.1);
-            zaccl[i]=(1);
-            wait(0.1);
-            
-            //formula
-        
-         totvect[i] = sqrt(((xaccl[i]-xavg)* (xaccl[i]-xavg))+ ((yaccl[i] - yavg)*(yaccl[i] - yavg)) + ((zaccl[i] - zavg)*(zaccl[i] - zavg)));
-         totave[i] = (totvect[i] + totvect[i-1]) / 2 ;
-         totvel[i]=(totave[i]*0.2);
- //totave1[i]=(totvel[i]+totvel[i-1])/2;
-         totdist[i]=(totvect[i]*(0.2*0.2)/2);
-         totheight[i]=sqrt((((xaccl[i]-xavg)* (xaccl[i]-xavg))+ ((yaccl[i] - yavg)*(yaccl[i] - yavg)))*(0.2*0.2)/2);
- //totave3[i]=((totheight[i]+totheight[i-1])/2);
-         acc=acc+totave[i];
-         
-         uart1.printf("%i\n", (int16_t)totave[i]);
-         uart1.printf("%i\n", (int16_t)totvel[i]);
-         uart1.printf("%i\n", (int16_t)totdist[i]);
-         uart1.printf("%i\n", (int16_t)totheight[i]);
-         
-         wait(0.1);
-         
-         if (steps == 10)
-         {
-             uart1.printf("capek");
-         }
-         
-         else
-         {
-           //nothing  
-         }
-         
-         //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("%i\n", (int16_t)steps);
-             
-    
-        }
-       */
-       
-       char buffer [20];
+        int xaccl[20];
+        int yaccl[20];
+        int zaccl[20];
        
        // Test Daata
        memset(&buffer, 0, sizeof(buffer));
-       int16_t reading_1 = 54;
-       int16_t reading_2 = 42;
-       int16_t reading_3 = 32;
+       int16_t perc     = 0;
+       int16_t kec      = 0;
+       int16_t jar      = 0;
+       int16_t ting     = 0;
+       snprintf(buffer, 20, "data: %d,%d,%d,%d\n",(int16_t)perc,(int16_t)kec,(int16_t)jar,(int16_t)ting);
        
-       snprintf(buffer,20,"data: %d, %d, %d\n", reading_1, reading_2, reading_3);
-         
-         
     led1 = 1;
     uart1.baud(9600);
     Ticker ticker;
     ticker.attach(periodicCallback, 1);
     
-    
-    
     DEBUG("Initialising the nRF51822\n\r");
     ble.init();
     ble.onDisconnection(disconnectionCallback);
@@ -228,18 +167,98 @@
 
     UARTService uartService(ble);
     uartServicePtr = &uartService;
- 
+    
+     uart1.printf("Starting ADXL345 test...\n");
+     wait(0.1);
+     uart1.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
+     wait(0.1);
+
+    
+    //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();
+     
+      
      while (1) 
      {
          ble.waitForEvent();
          wait(0.1);
          accelerometer.getOutput(readings);
          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]);
+         uart1.printf("%i, %i, %i\n", (int16_t)avg1, (int16_t)avg2, (int16_t)avg3);
+         wait(0.1);
+         
+         
          
-         ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
+         //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("acc: %i\n", (int16_t)totave[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "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("vel: %i\n", (int16_t)totvel[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "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("dist: %i\n", (int16_t)totdist[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "dist: %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("hght: %i\n\n", (int16_t)totheight[i]);
+            memset(&buffer, 0, sizeof(buffer));
+            snprintf(buffer, 20, "hght: %d\n\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);
+             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)steps, sizeof(steps), false);
+  
+             
+        }
     }
  
  }