main.cpp

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