main.cpp

00001 
00002 #include "mbed.h"
00003 #include "ble/BLE.h"
00004 
00005 #include "ble/services/UARTService.h"
00006 
00007 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
00008                                * it will have an impact on code-size and power consumption. */
00009 
00010 #if NEED_CONSOLE_OUTPUT
00011 #define DEBUG(...) { printf(__VA_ARGS__); }
00012 #else
00013 #define DEBUG(...) /* nothing */
00014 #endif /* #if NEED_CONSOLE_OUTPUT */
00015 
00016 BLEDevice  ble;
00017 DigitalOut led1(LED1);
00018 UARTService *uartServicePtr;
00019 Serial uart1(USBTX, USBRX);
00020 
00021 AnalogIn analog_value1(p4);
00022 AnalogIn analog_value2(p5);
00023 AnalogIn analog_value3(p6);
00024 
00025 DigitalOut led(LED1);
00026 
00027 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00028 {
00029     DEBUG("Disconnected!\n\r");
00030     DEBUG("Restarting the advertising process\n\r");
00031     ble.startAdvertising();
00032 }
00033 
00034 void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
00035    
00036     DEBUG("Connected!\n\r");
00037     
00038 }
00039 
00040 
00041 uint8_t b[40]={'a','d','q','w'};
00042 void onDataWritten1(const GattWriteCallbackParams *params)
00043 {
00044     
00045     
00046     if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
00047         uint16_t bytesRead = params->len;
00048         DEBUG("received %u bytes %s\n\r", bytesRead,params->data);
00049        
00050     }
00051 }
00052 
00053 
00054 void periodicCallback(void)
00055 {
00056     led1 = !led1;
00057     
00058 }
00059 
00060 int threshold =80;
00061 int xval[10]={0};
00062 int yval[10]={0};
00063 int zval[10]={0};
00064 int xavg;
00065 int yavg;
00066 int zavg;
00067 
00068 
00069 int calibratex()
00070 {
00071    led1=1;
00072    long int sum1  = 0;
00073    for (int i=0; i<10; i++)
00074    {
00075        
00076        xval[i] = analog_value1.read_u16();  //nilai x adxl335
00077        sum1 = yval[i]+sum1;
00078    }
00079    
00080    xavg=sum1/10.0;
00081 
00082    led1=0;     
00083    return xavg;
00084 }
00085 int calibratey()
00086 {
00087    led1=1;
00088    long int sum2 = 0;
00089    for (int i=0; i<10; i++)
00090    {
00091        
00092        yval[i] = analog_value2.read_u16(); //nilai y adxl335
00093        sum2 = yval[i]+sum2;
00094    }
00095    yavg=sum2/10.0;
00096    led1=0;     
00097    return yavg;
00098 }
00099 int calibratez()
00100 {
00101     
00102    led1=1;
00103    long int sum3 = 0;
00104    for (int i=0; i<10; i++)
00105    {
00106        
00107        zval[i]=analog_value3.read_u16(); //nilai z adxl335
00108        sum3 = zval[i]+sum3;
00109    }
00110    
00111    zavg=sum3/10.0;
00112    led1=0;     
00113    return zavg;
00114 }
00115 
00116 
00117 int main(void)
00118 {
00119     uart1.baud(9600);
00120     char buffer [20];
00121     
00122     printf("\nAnalogIn Example\n");
00123     
00124     led1 = 1;
00125     Ticker ticker;
00126     ticker.attach(periodicCallback, 1);
00127 
00128     DEBUG("Initialising the nRF51822\n\r");
00129     ble.init();
00130     ble.onDisconnection(disconnectionCallback);
00131     ble.onDataWritten(onDataWritten1);
00132 
00133     /* setup advertising */
00134     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00135     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00136     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00137                                      (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
00138     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00139                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00140 
00141     ble.setAdvertisingInterval(1000); /* 1000ms; in multiples of 0.625ms. */
00142     ble.startAdvertising();
00143 
00144     UARTService uartService(ble);
00145     uartServicePtr = &uartService;
00146     
00147     int avg1 = calibratex();
00148     int avg2 = calibratey();
00149     int avg3 = calibratez();
00150     
00151         //inisiliasi
00152         int steps=0;
00153         int flag=0;
00154         int acc=0;
00155         int totvect   [20] = {0};
00156         int totave    [20] = {0};
00157         int totvel    [20] = {0};
00158         int totdist   [20] = {0};
00159         int totheight [20] = {0};
00160             
00161         
00162         //float sum1, sum2, sum3 = 0
00163         int xaccl[20];
00164         int yaccl[20];
00165         int zaccl[20];
00166        
00167        // Test Daata
00168        memset(&buffer, 0, sizeof(buffer));
00169 /*       int16_t reading_1= 0;
00170        int16_t reading_2= 0;
00171        int16_t reading_3= 0;
00172        int16_t avg_1= 0;
00173        int16_t avg_2= 0;
00174        int16_t avg_3= 0;*/
00175        
00176        int16_t lang     = 0;
00177        int16_t perc     = 0;
00178        int16_t kec      = 0;
00179        int16_t jar      = 0;
00180        int16_t ting     = 0;
00181        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);
00182 /*       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);*/
00183 
00184     
00185 
00186     while (true) 
00187     {
00188         ble.waitForEvent();
00189         int16_t x = analog_value1.read_u16();
00190         int16_t y = analog_value2.read_u16();
00191         int16_t z = analog_value3.read_u16();
00192         uart1.printf("\r x = %d y = %d z = %d \n",(int16_t)x,(int16_t)y,(int16_t)z);
00193 /*         memset(&buffer, 0, sizeof(buffer));
00194          snprintf(buffer, 20, "data: %d,%d,%d\n", (int16_t)x,(int16_t)y,(int16_t)z);
00195          ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);*/
00196          
00197         uart1.printf("\n%i, %i, %i\n", (int16_t)avg1, (int16_t)avg2, (int16_t)avg3);
00198 /*         memset(&buffer, 0, sizeof(buffer));
00199          snprintf(buffer, 20, "data: %d,%d,%d\n\n", (int16_t)avg1,(int16_t)avg2,(int16_t)avg3);
00200          ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);*/
00201 
00202         wait(0.5);
00203         
00204         //float x,y,z
00205         for (int i=0; i<10; i++)
00206         {
00207             xaccl[i]=(analog_value1.read_u16());
00208             wait(0.1);
00209             yaccl[i]=(analog_value2.read_u16());
00210             wait(0.1);
00211             zaccl[i]=(analog_value3.read_u16());
00212             wait(0.1);
00213             
00214             //formula
00215             totvect[i] = sqrt((double)((xaccl[i]-xavg)* (xaccl[i]-xavg))+(double) ((yaccl[i] - yavg)*(yaccl[i] - yavg)) +(double) ((zaccl[i] - zavg)*(zaccl[i] - zavg)));
00216             totave[i] = (totvect[i] + totvect[i-1]) / 2 ;
00217             uart1.printf("\t acc: %i\n", (int16_t)totave[i]);
00218             memset(&buffer, 0, sizeof(buffer));
00219             snprintf(buffer, 20, "\t acc: %d\n", (int16_t)totave[i]);
00220             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00221 
00222             totvel[i]=(totave[i]*0.2);
00223             uart1.printf("\t vel: %i\n", (int16_t)totvel[i]);
00224             memset(&buffer, 0, sizeof(buffer));
00225             snprintf(buffer, 20, "\t vel: %d\n", (int16_t)totvel[i]);
00226             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00227 
00228             totdist[i]=(totvect[i]*(0.2*0.2)/2);
00229             uart1.printf("\t dist: %i\n", (int16_t)totdist[i]);
00230             memset(&buffer, 0, sizeof(buffer));
00231             snprintf(buffer, 20, "\tdist: %d\n", (int16_t)totdist[i]);
00232             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00233   
00234             totheight[i]=sqrt((((xaccl[i]-xavg)* (xaccl[i]-xavg))+ ((yaccl[i] - yavg)*(yaccl[i] - yavg)))*(0.2*0.2)/2);
00235             uart1.printf("\thght: %i\n", (int16_t)totheight[i]);
00236             memset(&buffer, 0, sizeof(buffer));
00237             snprintf(buffer, 20, "\thght: %d\n", (int16_t)totheight[i]);
00238             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00239   
00240             acc=acc+totave[i];
00241          
00242          
00243          
00244          //cal steps
00245          if (totave[i] > threshold && flag==0)
00246          {
00247              steps = steps+1;
00248              flag=1;
00249          }
00250          else if (totave[i] > threshold && flag == 1)
00251          {
00252           // do nothing   
00253          }
00254          if (totave[i] < threshold && flag == 1)
00255              {flag=0;}
00256              uart1.printf("steps: %i\n", (int16_t)steps);
00257              memset(&buffer, 0, sizeof(buffer));
00258              snprintf(buffer, 20, "\nsteps: %d\t", (int16_t)steps);
00259              ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00260              
00261         }
00262     }
00263     
00264 }