Diff: main.cpp

Revision:

15:34c3e96ce282

Parent:

14:4dfb61282efb

diff -r 4dfb61282efb -r 34c3e96ce282 main.cpp
--- a/main.cpp	Wed Oct 11 08:09:08 2017 +0000
+++ b/main.cpp	Sat Dec 09 06:25:35 2017 +0000
@@ -1,25 +1,10 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2013 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 
 #include "mbed.h"
 #include "ble/BLE.h"
 
 #include "ble/services/UARTService.h"
 
-#define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
+#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. */
 
 #if NEED_CONSOLE_OUTPUT
@@ -30,13 +15,12 @@
 
 BLEDevice  ble;
 DigitalOut led1(LED1);
-
 UARTService *uartServicePtr;
+Serial uart1(USBTX, USBRX);
 
-Serial pc(USBTX, USBRX);
-AnalogIn analog_value1(A0);
-AnalogIn analog_value2(A1);
-AnalogIn analog_value3(A2);
+AnalogIn analog_value1(p4);
+AnalogIn analog_value2(p5);
+AnalogIn analog_value3(p6);
 
 DigitalOut led(LED1);
 
@@ -47,23 +31,94 @@
     ble.startAdvertising();
 }
 
-void onDataWritten(const GattWriteCallbackParams *params)
+void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
+   
+    DEBUG("Connected!\n\r");
+    
+}
+
+
+uint8_t b[40]={'a','d','q','w'};
+void onDataWritten1(const GattWriteCallbackParams *params)
 {
+    
+    
     if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
         uint16_t bytesRead = params->len;
-        DEBUG("received %u bytes\n\r", bytesRead);
-        ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), params->data, bytesRead);
+        DEBUG("received %u bytes %s\n\r", bytesRead,params->data);
+       
     }
 }
 
+
 void periodicCallback(void)
 {
     led1 = !led1;
+    
 }
 
+int threshold =80;
+int xval[10]={0};
+int yval[10]={0};
+int zval[10]={0};
+int xavg;
+int yavg;
+int zavg;
+
+
+int calibratex()
+{
+   led1=1;
+   long int sum1  = 0;
+   for (int i=0; i<10; i++)
+   {
+       
+       xval[i] = analog_value1.read_u16();  //nilai x adxl335
+       sum1 = yval[i]+sum1;
+   }
+   
+   xavg=sum1/10.0;
+
+   led1=0;     
+   return xavg;
+}
+int calibratey()
+{
+   led1=1;
+   long int sum2 = 0;
+   for (int i=0; i<10; i++)
+   {
+       
+       yval[i] = analog_value2.read_u16(); //nilai y adxl335
+       sum2 = yval[i]+sum2;
+   }
+   yavg=sum2/10.0;
+   led1=0;     
+   return yavg;
+}
+int calibratez()
+{
+    
+   led1=1;
+   long int sum3 = 0;
+   for (int i=0; i<10; i++)
+   {
+       
+       zval[i]=analog_value3.read_u16(); //nilai z adxl335
+       sum3 = zval[i]+sum3;
+   }
+   
+   zavg=sum3/10.0;
+   led1=0;     
+   return zavg;
+}
+
+
 int main(void)
 {
-    int x,y,z;
+    uart1.baud(9600);
+    char buffer [20];
+    
     printf("\nAnalogIn Example\n");
     
     led1 = 1;
@@ -73,7 +128,7 @@
     DEBUG("Initialising the nRF51822\n\r");
     ble.init();
     ble.onDisconnection(disconnectionCallback);
-    ble.onDataWritten(onDataWritten);
+    ble.onDataWritten(onDataWritten1);
 
     /* setup advertising */
     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
@@ -88,16 +143,122 @@
 
     UARTService uartService(ble);
     uartServicePtr = &uartService;
+    
+    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};
+            
+        
+        //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 (true) 
     {
         ble.waitForEvent();
-        x = analog_value1.read_u16();
-        y = analog_value2.read_u16();
-        z = analog_value3.read_u16();
-        printf("\r x = %d y = %d z = %d \n",x,y,z);
+        int16_t x = analog_value1.read_u16();
+        int16_t y = analog_value2.read_u16();
+        int16_t z = analog_value3.read_u16();
+        uart1.printf("\r x = %d y = %d z = %d \n",(int16_t)x,(int16_t)y,(int16_t)z);
+/*         memset(&buffer, 0, sizeof(buffer));
+         snprintf(buffer, 20, "data: %d,%d,%d\n", (int16_t)x,(int16_t)y,(int16_t)z);
+         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.5);
+        
+        //float x,y,z
+        for (int i=0; i<10; i++)
+        {
+            xaccl[i]=(analog_value1.read_u16());
+            wait(0.1);
+            yaccl[i]=(analog_value2.read_u16());
+            wait(0.1);
+            zaccl[i]=(analog_value3.read_u16());
+            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);
+             
+        }
     }
     
-    
 }