main.cpp

00001 #include "mbed.h"
00002 #include "ADXL345_I2C.h"
00003 #include "ble/BLE.h"
00004 #include "ble/services/UARTService.h"
00005 #include "Serial.h"
00006 #include <math.h>
00007 #include <stdlib.h>
00008 #define MAX 20
00009 #define k 4
00010 
00011 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
00012                                * it will have an impact on code-size and power consumption. */
00013 
00014 #if NEED_CONSOLE_OUTPUT
00015 #define DEBUG(...) { printf(__VA_ARGS__); }
00016 #else
00017 #define DEBUG(...) /* nothing */
00018 #endif /* #if NEED_CONSOLE_OUTPUT */
00019 
00020  ADXL345_I2C accelerometer(p30, p7);
00021  BLEDevice  ble;
00022  DigitalOut led1(LED1);
00023  Serial uart1(USBTX,USBRX);
00024  UARTService *uartServicePtr;
00025  
00026  
00027  
00028  
00029  void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00030 {
00031     DEBUG("Disconnected!\n\r");
00032     DEBUG("Restarting the advertising process\n\r");
00033     ble.startAdvertising();
00034 }
00035 
00036 void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
00037    
00038     DEBUG("Connected!\n\r");
00039     
00040 }
00041 
00042 uint8_t b[40]={'a','d','q','w'};
00043 void onDataWritten(const GattWriteCallbackParams *params)
00044 {
00045     if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
00046         uint16_t bytesRead = params->len;
00047         DEBUG("received %u bytes %s\n\r", bytesRead,params->data);
00048     }
00049 }
00050 
00051 void periodicCallback(void)
00052 {
00053     led1 = !led1;
00054 }
00055 
00056 int threshold =6.8;
00057 int xval[10]={0};
00058 int yval[10]={0};
00059 int zval[10]={0};
00060 
00061 using namespace std;
00062 enum category{STANDING,CLIMBING,WALKING};
00063 class data{
00064     int x,y;
00065     category cat;
00066     public:
00067     void setd(int a,int b,category c){
00068         x=a;
00069         y=b;
00070         cat=c;
00071     }
00072     int getx(){return x;}
00073     int gety(){return y;}
00074     category getcat(){
00075         return cat;
00076     }
00077 };//end of class
00078 int dis(data d1,data d2)
00079 {
00080     return sqrt(pow(((double)d2.getx()-(double)d1.getx()),2.0)+pow(((double)d2.gety()-(double)d1.gety()),2.0));
00081 }
00082 
00083  int main() 
00084  {
00085      uart1.baud(9600);
00086      int readings[3] = {0, 0, 0};
00087      char buffer [20];
00088      
00089         //inisiliasi
00090         int steps=0;
00091         int flag=0;
00092         int acc=0;
00093         int totvect   [20] = {0};
00094         int totave    [20] = {0};
00095         int totvel    [20] = {0};
00096         int totdist   [20] = {0};
00097         
00098             
00099         
00100         //float sum1, sum2, sum3 = 0
00101         int xaccl[20];
00102         int yaccl[20];
00103         int zaccl[20];
00104        
00105        // Test Daata
00106        memset(&buffer, 0, sizeof(buffer));
00107        int16_t reading_1= 0;
00108        int16_t reading_2= 0;
00109        int16_t reading_3= 0;
00110        int16_t avg_1= 0;
00111        int16_t avg_2= 0;
00112        int16_t avg_3= 0;
00113        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);
00114        
00115        int p,q; //input
00116        int a[MAX]; //store distances
00117        int b[k]; //to get min distances, used in calc
00118        int c[k]; //to store freq
00119 
00120        
00121     led1 = 1;
00122     uart1.baud(9600);
00123     Ticker ticker;
00124     ticker.attach(periodicCallback, 1);
00125     
00126     DEBUG("Initialising the nRF51822\n\r");
00127     ble.init();
00128     ble.onDisconnection(disconnectionCallback);
00129     ble.onConnection(connectionCallback);
00130     ble.onDataWritten(onDataWritten);
00131 
00132     /* setup advertising */
00133     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00134     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00135     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00136                                      (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
00137     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00138                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00139 
00140     ble.setAdvertisingInterval(1000); /* 1000ms; in multiples of 0.625ms. */
00141     ble.startAdvertising();
00142 
00143     UARTService uartService(ble);
00144     uartServicePtr = &uartService;
00145     
00146      uart1.printf("Starting ADXL345 test...\n");
00147      wait(0.1);
00148      uart1.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
00149      wait(0.1);
00150 
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        
00165       
00166      while (1) 
00167      {
00168          ble.waitForEvent();
00169          wait(0.1);
00170          accelerometer.getOutput(readings);
00171          /*uart1.printf("\n%i, %i, %i\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
00172          memset(&buffer, 0, sizeof(buffer));
00173          snprintf(buffer, 20, "data: %d,%d,%d\n\n", (int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[0]);
00174          ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);*/
00175          
00176          
00177          //float x,y,z
00178         for (int i=0; i<10; i++)
00179         {
00180             xaccl[i]=(readings[0]);
00181             wait(0.1);
00182             yaccl[i]=(readings[1]);
00183             wait(0.1);
00184             zaccl[i]=(readings[2]);
00185             wait(0.1);
00186             
00187             
00188             //formula
00189             totvect[i] = sqrt(pow((double)xaccl[i],2.0)+pow((double)yaccl[i],2.0)+pow((double)zaccl[i],2.0));
00190             wait(0.3);
00191             totave[i] = ((double)totvect[i]+(double)totvect[i-1])/2.0 ;
00192             wait(0.3);
00193             acc=acc+totave[i];
00194             wait(0.3);
00195             totvel[i]=(1*((double)totave[i]-(double)totave[i-1]/2.0)+totvel[i-1]);
00196             wait(0.3);
00197             totdist[i]=(1*((double)totvel[i]-(double)totvel[i-1]/2.0)+totdist[i-1]);         
00198          
00199          
00200          //cal steps
00201          if (totave[i] > threshold && flag==0)
00202          {
00203              steps = steps+1;
00204              flag=1;
00205          }
00206          else if (totave[i] > threshold && flag == 1)
00207          {
00208           // do nothing   
00209          }
00210          if (totave[i] < threshold && flag == 1)
00211              {flag=0;}
00212              uart1.printf("\nsteps:%i", (int16_t)steps);
00213              memset(&buffer, 0, sizeof(buffer));
00214              snprintf(buffer, 20, "\n%d\t", (int16_t)steps);
00215              ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00216              
00217             uart1.printf("\tacc:%i", (int16_t)totave[i]);
00218             memset(&buffer, 0, sizeof(buffer));
00219             snprintf(buffer, 20, "\t%d", (int16_t)totave[i]);
00220             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00221         
00222             uart1.printf("\tvel:%i", (int16_t)totvel[i]);
00223             memset(&buffer, 0, sizeof(buffer));
00224             snprintf(buffer, 20, "\t%d", (int16_t)totvel[i]);
00225             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00226 
00227             uart1.printf("\tdist:%i", (int16_t)totdist[i]);
00228             memset(&buffer, 0, sizeof(buffer));
00229             snprintf(buffer, 20, "\t%d\n", (int16_t)totdist[i]);
00230             ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)buffer, sizeof(buffer), false);
00231   
00232       
00233         }
00234        
00235        do{
00236         wait(0.1);
00237         
00238         for(int i=0;i<k;i++)
00239         { //initiLIZATION
00240             b[i]=-1;
00241             c[i]=0;
00242         } 
00243         int min=1000;
00244         p = (readings[0]);
00245         q = (readings[1]);
00246         //uart1.scanf("%ld,%ld",&p,&q);
00247         if((p < 0) | (q < 0))
00248             exit(0);
00249         data n; //data point to classify
00250         n.setd(p,q,STANDING);
00251         data d[MAX]; //training set
00252         
00253         d[0].setd(1,1,STANDING);
00254         d[1].setd(1,2,STANDING);
00255         d[2].setd(1,3,STANDING);
00256         d[3].setd(1,4,STANDING);
00257         d[4].setd(1,5,STANDING);
00258         d[5].setd(1,6,STANDING);
00259         d[6].setd(1,7,STANDING);
00260         d[7].setd(2,1,STANDING);
00261         d[8].setd(2,2,STANDING);
00262         d[9].setd(2,3,WALKING);
00263         d[10].setd(2,4,WALKING);
00264         d[11].setd(2,5,WALKING);
00265         d[12].setd(2,6,WALKING);
00266         d[13].setd(2,7,WALKING);
00267         d[14].setd(5,1,CLIMBING);
00268         d[15].setd(5,2,CLIMBING);
00269         d[16].setd(5,3,CLIMBING);
00270         d[17].setd(5,4,CLIMBING);
00271         d[18].setd(5,5,CLIMBING);
00272         d[19].setd(5,6,CLIMBING);
00273         for(int i=0;i<20;i++){
00274             a[i]=dis(n,d[i]);
00275             //uart1.printf("\t\t %d\n", a[i]);
00276         }
00277         //k-nearest neighbours calculation i.e smallest k distances
00278         for(int j=0;j<k;j++)
00279         {
00280             min=1000;
00281             for(int i=0;i<20;i++)
00282             {
00283                 if(i!=b[0]&&i!=b[1]&&i!=b[2])
00284                 { 
00285                     if((a[i]<=min))
00286                     {
00287                         min=a[i];
00288                         b[j]=i;
00289                     }
00290                 }
00291             } 
00292             
00293             //uart1.printf("%d\n",min);
00294         }
00295         //counting frequency of a class in each neighbour   
00296         for(int i=0;i<k;i++)
00297         {
00298             switch (d[b[i]].getcat())
00299             { 
00300             case STANDING:
00301                 c[0]++;
00302                 break;
00303             case WALKING:
00304                 c[2]++;
00305                 break;
00306             case CLIMBING:
00307                 c[1]++;
00308                 break;
00309             }
00310         } //counting max frequency
00311         int max=-1,j;
00312         for(int i=0;i<k;i++)
00313         {    
00314             if(c[i]>max){
00315                 max=c[i];
00316                 j=i;
00317             }
00318         }
00319        
00320             wait(0.1);
00321             printf("Prediction is:");
00322             switch (j)
00323             {    
00324             case 0:
00325                 uart1.printf("STANDING\n");
00326                 break;
00327             case 1:
00328                 uart1.printf("CLIMBING\n");
00329                 break;
00330             case 2:
00331                 uart1.printf("WALKING\n");
00332                 break;
00333             }
00334          
00335         
00336     }while(true); 
00337         
00338     }
00339  
00340  }
00341