main.cpp

00001 /*
00002 
00003 Copyright (c) 2012-2014 RedBearLab
00004 
00005 Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
00006 and associated documentation files (the "Software"), to deal in the Software without restriction, 
00007 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 
00008 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, 
00009 subject to the following conditions:
00010 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
00011 
00012 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
00013 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 
00014 PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE 
00015 FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 
00016 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017 
00018 */
00019 
00020 /*
00021  *    The application works with the BLEController iOS/Android App.
00022  *    Type something from the Terminal to send
00023  *    to the BLEController App or vice verse.
00024  *    Characteristics received from App will print on Terminal.
00025  */
00026  
00027 #include "mbed.h"
00028 #include "ble/BLE.h"
00029 #include "LIS3DH.h"
00030 #include "strike.h"
00031 #include "Sensor.h"
00032 
00033 #define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
00034 #define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
00035 #define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */
00036 
00037 #define TXRX_BUF_LEN                     20
00038 
00039 
00040 //you can receive less then 93 bytes at 921600 Bd
00041 #define UART_SENSOR_COMMAND_BUFFER_SIZE (100)
00042 #define SYSTEM_COM_BUFFER   (1024)
00043 #define RADIO_MESSAGE_LEN   (64)
00044 #define VERSION "alpha_20150409-01"
00045 #define SPEW (0)
00046 #define STRIKE_DETECT_1 (1)
00047 #define STRIKE_DETECT_2 (2)
00048 
00049 #define DEFAULT_TIMESTEP (10) // in milliseconds
00050 //#define DEFAULT_FAST_FILTER (650)
00051 //#define DEFAULT_SLOW_FILTER (975)
00052 #define DEFAULT_BIG_HIT_THRESHOLD (3000)
00053 #define DEFAULT_SMALL_HIT_THRESHOLD (200)
00054 #define DEFAULT_JERK_THRESHOLD (500)
00055 
00056 #define DEFAULT_C1_1 (0.35)
00057 #define DEFAULT_C2_1 (0.65)
00058 #define DEFAULT_C1_2 (0.025)
00059 #define DEFAULT_C2_2 (0.975)
00060 
00061 #define DEFAULT_SHADOW_DECAY (0.8)
00062 
00063 BLE  ble;
00064 
00065 Serial pc(P0_4, USBRX);
00066 
00067 
00068 // The Nordic UART Service
00069 static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00070 static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00071 static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
00072 static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
00073 
00074 
00075 uint8_t txPayload[TXRX_BUF_LEN] = {0,};
00076 uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
00077 
00078 static uint8_t rx_buf[TXRX_BUF_LEN];
00079 static uint8_t rx_len=0;
00080 
00081 
00082 GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00083                                       
00084 GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00085                                       
00086 GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
00087 
00088 GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
00089 
00090 
00091 
00092 void interpret(char parameter, int value);
00093 void getRadioInput(char *data, uint16_t size);
00094 void read_accel();
00095 
00096 Ticker measure;
00097 Ticker spew;
00098 uint16_t strike_value = 0;
00099 int last_result = 0;
00100 bool new_strike = false;
00101 bool active = false;
00102 static volatile int config_parameter = 0;
00103 
00104 static volatile int mode = STRIKE_DETECT_2;
00105 static volatile int timestep = DEFAULT_TIMESTEP;
00106 //static volatile int fast_filter = DEFAULT_FAST_FILTER;
00107 //static volatile int slow_filter = DEFAULT_SLOW_FILTER;
00108 static volatile int big_hit_threshold = DEFAULT_BIG_HIT_THRESHOLD;
00109 static volatile int small_hit_threshold = DEFAULT_SMALL_HIT_THRESHOLD;
00110 static volatile int jerk_threshold = DEFAULT_JERK_THRESHOLD;
00111 
00112 static volatile float c1_1 = DEFAULT_C1_1;
00113 static volatile float c2_1 = DEFAULT_C2_1;
00114 static volatile float c1_2 = DEFAULT_C1_2;
00115 static volatile float c2_2 = DEFAULT_C2_2;
00116 
00117 static volatile float shadow_decay = DEFAULT_SHADOW_DECAY;
00118 
00119 static int16_t xzy[3];
00120 
00121 
00122 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00123 {
00124     pc.printf("Disconnected \r\n");
00125     pc.printf("Restart advertising \r\n");
00126     ble.startAdvertising();
00127 }
00128 
00129 void WrittenHandler(const GattWriteCallbackParams *Handler)
00130 {   
00131     uint8_t buf[TXRX_BUF_LEN];
00132     uint16_t bytesRead, index;
00133     
00134     if (Handler->handle == txCharacteristic.getValueAttribute().getHandle()) 
00135     {
00136         ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
00137         memset(txPayload, 0, TXRX_BUF_LEN);
00138         memcpy(txPayload, buf, TXRX_BUF_LEN);       
00139         pc.printf("WriteHandler \r\n");
00140         pc.printf("Length: %d\r\n", bytesRead);
00141         pc.printf("Data: ");
00142         for(index=0; index<bytesRead; index++)
00143         {
00144             pc.putc(txPayload[index]);        
00145         }
00146         pc.printf("\r\n");
00147         
00148         getRadioInput((char*)txPayload, bytesRead);
00149     }
00150 }
00151 
00152 void uartCB(void)
00153 {   
00154     while(pc.readable())    
00155     {
00156         rx_buf[rx_len++] = pc.getc();    
00157         if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n')
00158         {
00159             ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); 
00160             pc.printf("RecHandler \r\n");
00161             pc.printf("Length: ");
00162             pc.putc(rx_len);
00163             pc.printf("\r\n");
00164             rx_len = 0;
00165             break;
00166         }
00167     }
00168 }
00169 
00170 int main(void)
00171 {
00172     ble.init();
00173     ble.onDisconnection(disconnectionCallback);
00174     ble.onDataWritten(WrittenHandler);  
00175     
00176     pc.baud(9600);
00177     pc.printf("TenCount \r\n");
00178     
00179     LIS3DH_init();
00180     
00181     //pc.attach( uartCB , pc.RxIrq);
00182    // setup advertising 
00183     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00184     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00185     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00186                                     (const uint8_t *)"TenCount", sizeof("TenCount") - 1);
00187     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00188                                     (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
00189     // 100ms; in multiples of 0.625ms. 
00190     ble.setAdvertisingInterval(160);
00191 
00192     ble.addService(uartService);
00193     
00194     ble.startAdvertising(); 
00195     pc.printf("Advertising Start \r\n");
00196     
00197     char txbuff[40] = {0};
00198     
00199     while(1)
00200     {
00201         //ble.waitForEvent(); 
00202          if(new_strike){
00203             if(mode == SPEW){
00204                 pc.printf("s%u\r\n", strike_value);   
00205             }
00206             else{
00207                 snprintf(txbuff, sizeof(txbuff), "s%u\r\n", strike_value);
00208                 pc.printf("%s", txbuff);  
00209                 ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)txbuff, strlen(txbuff)); 
00210                 new_strike = false;
00211             }
00212         }
00213     }
00214 }
00215 
00216 void strike_detect1(){
00217     //static int16_t xzy[3];
00218     static Sensor* s = new Sensor();
00219 
00220     LIS3DH_readAll(xzy);
00221     //algorithm expects -/+8g, 0-4095, accelerometer reports ??? (+/-16g, 0-65535?)
00222     int result = parseXY(xzy[0]/16 + 2048, xzy[2]/16 + 2048, s);
00223     if(result){ 
00224         //strike_timer.reset();
00225         //strike_timer.start();
00226         strike_detect_reset(s);
00227         result = int((float)result*(10000.0/65535.0));
00228         new_strike = true;
00229         last_result = result;
00230         strike_value = result;
00231     }
00232 }
00233 
00234 void strike_detect2(){
00235     static float low_pass1 = 0.0;
00236     static float low_pass2 = 0.0;
00237     float RMS = 0.0;
00238     static float last_decay = 0.0;
00239     float decay = 0.0;
00240     static float accumulator = 0.0;
00241     bool accumulating = false;
00242     float jerk = 0.0;
00243     static float shadow = 0.0;
00244 
00245     LIS3DH_readAll(xzy);
00246     float t1 = xzy[0];
00247     float t2 = xzy[2];
00248     RMS = sqrt(t1*t1 + t2*t2);
00249 
00250     jerk = low_pass1;
00251     low_pass1 = c1_1*RMS + c2_1*low_pass1;
00252     low_pass2 = c1_2*RMS + c2_2*low_pass2;
00253 
00254     jerk = low_pass1 - jerk;
00255     if(jerk > jerk_threshold)accumulating = true;
00256 
00257     decay = low_pass1 - low_pass2;
00258     if(decay < 0) decay = 0;
00259     
00260     shadow *= shadow_decay;
00261 
00262     if(decay < 400){
00263       strike_value = ((long)accumulator/30);
00264       if(strike_value > small_hit_threshold && strike_value > shadow){
00265           new_strike = true;
00266           pc.printf("s%ld\r", (long)accumulator/30);
00267           if(strike_value > big_hit_threshold) shadow = (float) strike_value;
00268       }
00269       accumulator = 0;
00270       accumulating = false;
00271     }
00272     else if(accumulating)accumulator += decay;
00273     last_decay = decay;
00274 }
00275 
00276 void spew_accel_old(){
00277     static int counter = 0;
00278     static float low_pass = 0.0;
00279     float RMS = 0.0;
00280     float delta = 0.2;
00281     float decay = 0.0;
00282     
00283     //static int16_t xzy[3];
00284     LIS3DH_readAll(xzy);
00285     RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
00286     low_pass = delta*RMS + (1-delta)*low_pass;
00287     
00288     if(RMS > decay)decay = low_pass;
00289     decay = decay * 0.95;
00290     
00291     //UART.printf("%d %d %d\r\n", last_result, xzy[0], xzy[2]);  // should not be printing in interrupt, am going to hell
00292     pc.printf("%f, %f, %f\r\n", RMS, low_pass, decay);  // should not be printing in interrupt, am going to hell
00293     if(counter > 15){
00294         last_result = 0;
00295         counter = 0;
00296     }
00297     if(last_result)counter++;
00298 }
00299 /*
00300 void spew_accel(){
00301     static float low_pass1 = 0.0;
00302     static float low_pass2 = 0.0;
00303     float RMS = 0.0;
00304     static float last_decay = 0.0;
00305     float decay = 0.0;
00306     static float accumulator = 0.0;
00307     bool accumulating = false;
00308     float jerk = 0.0;
00309     //static float biggest_jerk = 0.0;
00310     
00311     //static int16_t xzy[3];
00312     LIS3DH_readAll(xzy);
00313     //RMS = (float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2];
00314     float t1 = xzy[0];
00315     float t2 = xzy[2];
00316     RMS = sqrt(t1*t1 + t2*t2);
00317     //RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
00318     jerk = low_pass1;
00319     low_pass1 = 0.35*RMS + 0.65*low_pass1;
00320     low_pass2 = 0.025*RMS + 0.975*low_pass2;
00321 
00322     jerk = low_pass1 - jerk;
00323     if(jerk > 500)accumulating = true;
00324     //if(jerk > biggest_jerk)biggest_jerk = jerk;
00325     
00326     //if(RMS > decay)decay = RMS;
00327     //decay = decay * 0.9999;
00328     decay = low_pass1 - low_pass2;
00329     if(decay < 0) decay = 0;
00330 
00331     if(decay < 400){
00332       if(accumulator > 10000)UART.printf("s%ld\r", (long)accumulator/30);
00333       accumulator = 0;
00334       accumulating = false;
00335       //biggest_jerk = 0;
00336     }
00337     else if(accumulating)accumulator += decay;
00338     last_decay = decay;
00339     
00340 }*/
00341 
00342 void getRadioInput(char *data, uint16_t length)
00343 {
00344   static int i = 0;
00345   static char parameter = '_';
00346   static char buffer[RADIO_MESSAGE_LEN + 1];
00347   int value = 0;
00348   
00349   // listen for commands coming over bluetooth
00350   for (int j = 0; j < length; ++j){
00351     char ch = data[j];
00352 
00353     pc.printf("looping: %c. %c\r\n", ch, parameter);
00354 
00355     // if ch is a message terminator, if parameter is anything other than '_', process the message
00356     if((ch == '\r' || ch == ';' || ch == '\n'))
00357     {
00358       if(i > 0  && parameter != '_')
00359       {
00360         buffer[i-1] = 0;
00361         value = atoi(buffer);
00362         
00363         interpret(parameter, value);
00364       }
00365 
00366       //  done sending, reset parameters
00367       parameter = '_';
00368       buffer[0] = 0;
00369       i=0;
00370     }
00371     else
00372     { // got anything but a message terminator
00373       if(i==0)
00374         parameter = ch; // nothing accumulated yet
00375       else 
00376         buffer[i-1] = ch;
00377         
00378       i++;
00379     }
00380     //UART.printf("%c: %s, %d\r\n",parameter, buffer, i);
00381   }
00382 }
00383 
00384 void interpret(char parameter, int value){
00385     
00386     switch(parameter){
00387         case 'g':
00388             if(value == 1){
00389                 new_strike = false;
00390                 //active = true;
00391                 if(mode == STRIKE_DETECT_1)measure.attach_us(&strike_detect1, timestep*1000);
00392                 if(mode == STRIKE_DETECT_2)measure.attach_us(&strike_detect2, timestep*1000);
00393             }
00394             if(value == 0)measure.detach(); //active = false;
00395         break;
00396         case 'm':
00397             mode = value;
00398         break;
00399         case 'p':
00400             config_parameter = value;
00401             //UART.printf("parameter is: %d\r\n", value);
00402         break;
00403         case 'v':
00404             if(config_parameter < 8)set_setting(config_parameter, value);
00405             else{
00406                 //UART.printf("value is: %d\r\n", value);
00407                 if(config_parameter == 101){
00408                     //fast_filter = value;
00409                     c1_1 = (float)value/1000.0;
00410                     c2_1 = 1.0 - c1_1;
00411                 }
00412                 if(config_parameter == 102){
00413                     //slow_filter = value;
00414                     c1_2 = (float)value/1000.0;
00415                     c2_2 = 1.0 - c1_2;
00416                 }
00417                 if(config_parameter == 103)big_hit_threshold = value;
00418                 if(config_parameter == 104)small_hit_threshold = value;
00419                 if(config_parameter == 105)jerk_threshold = value;
00420                 if(config_parameter == 106){
00421                     float x = value;
00422                     shadow_decay = 0.9 + x*0.0001;
00423                     //UART.printf("shadow_decay is: %f\r\n", shadow_decay);
00424                 }
00425             }
00426         break;
00427         case 's':
00428             if(value > 0){
00429                 //measure.attach_us(&spew_accel, 1000);
00430                 //spew.attach(&spew_accel, float(value)/1000);
00431                 //measure.attach_us(&read_accel, 1000);
00432             }
00433             if(value == 0)spew.detach(); //active = false;
00434         break;
00435         case 't':
00436             //y(k) = a * y(k-1) + (1-a) * x(k)
00437             //a = exp (-T/tau)
00438             break;
00439         case 'z':
00440             pc.printf(VERSION);
00441         break;       
00442         default:
00443         // print stuff
00444         break;
00445     }
00446 }
00447 
00448 
00449 
00450 
00451 
00452 
00453 
00454 
00455 
00456 
00457 
00458 
00459 
00460 
00461 
00462