Diff: main.cpp

Revision:

4:2b0947ed0e0d

Parent:

3:b3f6c612b603

Child:

5:7c5630442028

--- a/main.cpp	Thu Jan 07 02:46:54 2016 +0000
+++ b/main.cpp	Sun Apr 24 15:02:56 2016 +0000
@@ -26,7 +26,9 @@
  
 #include "mbed.h"
 #include "ble/BLE.h"
-
+#include "LIS3DH.h"
+#include "strike.h"
+#include "Sensor.h"
 
 #define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
 #define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
@@ -34,6 +36,30 @@
 
 #define TXRX_BUF_LEN                     20
 
+
+//you can receive less then 93 bytes at 921600 Bd
+#define UART_SENSOR_COMMAND_BUFFER_SIZE (100)
+#define SYSTEM_COM_BUFFER   (1024)
+#define RADIO_MESSAGE_LEN   (64)
+#define VERSION "alpha_20150409-01"
+#define SPEW (0)
+#define STRIKE_DETECT_1 (1)
+#define STRIKE_DETECT_2 (2)
+
+#define DEFAULT_TIMESTEP (10) // in milliseconds
+//#define DEFAULT_FAST_FILTER (650)
+//#define DEFAULT_SLOW_FILTER (975)
+#define DEFAULT_BIG_HIT_THRESHOLD (3000)
+#define DEFAULT_SMALL_HIT_THRESHOLD (200)
+#define DEFAULT_JERK_THRESHOLD (500)
+
+#define DEFAULT_C1_1 (0.35)
+#define DEFAULT_C2_1 (0.65)
+#define DEFAULT_C1_2 (0.025)
+#define DEFAULT_C2_2 (0.975)
+
+#define DEFAULT_SHADOW_DECAY (0.8)
+
 BLE  ble;
 
 Serial pc(USBTX, USBRX);
@@ -63,6 +89,36 @@
 
 
 
+void interpret(char parameter, int value);
+void getRadioInput(char *data, uint16_t size);
+void read_accel();
+
+Ticker measure;
+Ticker spew;
+uint16_t strike_value = 0;
+int last_result = 0;
+bool new_strike = false;
+bool active = false;
+static volatile int config_parameter = 0;
+
+static volatile int mode = STRIKE_DETECT_2;
+static volatile int timestep = DEFAULT_TIMESTEP;
+//static volatile int fast_filter = DEFAULT_FAST_FILTER;
+//static volatile int slow_filter = DEFAULT_SLOW_FILTER;
+static volatile int big_hit_threshold = DEFAULT_BIG_HIT_THRESHOLD;
+static volatile int small_hit_threshold = DEFAULT_SMALL_HIT_THRESHOLD;
+static volatile int jerk_threshold = DEFAULT_JERK_THRESHOLD;
+
+static volatile float c1_1 = DEFAULT_C1_1;
+static volatile float c2_1 = DEFAULT_C2_1;
+static volatile float c1_2 = DEFAULT_C1_2;
+static volatile float c2_2 = DEFAULT_C2_2;
+
+static volatile float shadow_decay = DEFAULT_SHADOW_DECAY;
+
+static int16_t xzy[3];
+
+
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     pc.printf("Disconnected \r\n");
@@ -81,15 +137,15 @@
         memset(txPayload, 0, TXRX_BUF_LEN);
         memcpy(txPayload, buf, TXRX_BUF_LEN);       
         pc.printf("WriteHandler \r\n");
-        pc.printf("Length: ");
-        pc.putc(bytesRead);
-        pc.printf("\r\n");
+        pc.printf("Length: %d\r\n", bytesRead);
         pc.printf("Data: ");
         for(index=0; index<bytesRead; index++)
         {
             pc.putc(txPayload[index]);        
         }
         pc.printf("\r\n");
+        
+        getRadioInput((char*)txPayload, bytesRead);
     }
 }
 
@@ -118,14 +174,16 @@
     ble.onDataWritten(WrittenHandler);  
     
     pc.baud(9600);
-    pc.printf("SimpleChat Init \r\n");
+    pc.printf("TenCount \r\n");
+    
+    LIS3DH_init();
     
     pc.attach( uartCB , pc.RxIrq);
    // setup advertising 
     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
-                                    (const uint8_t *)"Biscuit", sizeof("Biscuit") - 1);
+                                    (const uint8_t *)"TenCount", sizeof("TenCount") - 1);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                     (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
     // 100ms; in multiples of 0.625ms. 
@@ -136,9 +194,254 @@
     ble.startAdvertising(); 
     pc.printf("Advertising Start \r\n");
     
+    char txbuff[40] = {0};
+    
     while(1)
     {
-        ble.waitForEvent(); 
+        //ble.waitForEvent(); 
+         if(new_strike){
+            if(mode == SPEW){
+                pc.printf("s%u\r\n", strike_value);   
+            }
+            else{
+                snprintf(txbuff, sizeof(txbuff), "s%u\r\n", strike_value);
+                pc.printf("%s", txbuff);  
+                ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)txbuff, strlen(txbuff)); 
+                new_strike = false;
+            }
+        }
+    }
+}
+
+void strike_detect1(){
+    //static int16_t xzy[3];
+    static Sensor* s = new Sensor();
+
+    LIS3DH_readAll(xzy);
+    //algorithm expects -/+8g, 0-4095, accelerometer reports ??? (+/-16g, 0-65535?)
+    int result = parseXY(xzy[0]/16 + 2048, xzy[2]/16 + 2048, s);
+    if(result){ 
+        //strike_timer.reset();
+        //strike_timer.start();
+        strike_detect_reset(s);
+        result = int((float)result*(10000.0/65535.0));
+        new_strike = true;
+        last_result = result;
+        strike_value = result;
+    }
+}
+
+void strike_detect2(){
+    static float low_pass1 = 0.0;
+    static float low_pass2 = 0.0;
+    float RMS = 0.0;
+    static float last_decay = 0.0;
+    float decay = 0.0;
+    static float accumulator = 0.0;
+    bool accumulating = false;
+    float jerk = 0.0;
+    static float shadow = 0.0;
+
+    LIS3DH_readAll(xzy);
+    float t1 = xzy[0];
+    float t2 = xzy[2];
+    RMS = sqrt(t1*t1 + t2*t2);
+
+    jerk = low_pass1;
+    low_pass1 = c1_1*RMS + c2_1*low_pass1;
+    low_pass2 = c1_2*RMS + c2_2*low_pass2;
+
+    jerk = low_pass1 - jerk;
+    if(jerk > jerk_threshold)accumulating = true;
+
+    decay = low_pass1 - low_pass2;
+    if(decay < 0) decay = 0;
+    
+    shadow *= shadow_decay;
+
+    if(decay < 400){
+      strike_value = ((long)accumulator/30);
+      if(strike_value > small_hit_threshold && strike_value > shadow){
+          new_strike = true;
+          pc.printf("s%ld\r", (long)accumulator/30);
+          if(strike_value > big_hit_threshold) shadow = (float) strike_value;
+      }
+      accumulator = 0;
+      accumulating = false;
+    }
+    else if(accumulating)accumulator += decay;
+    last_decay = decay;
+}
+
+void spew_accel_old(){
+    static int counter = 0;
+    static float low_pass = 0.0;
+    float RMS = 0.0;
+    float delta = 0.2;
+    float decay = 0.0;
+    
+    //static int16_t xzy[3];
+    LIS3DH_readAll(xzy);
+    RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
+    low_pass = delta*RMS + (1-delta)*low_pass;
+    
+    if(RMS > decay)decay = low_pass;
+    decay = decay * 0.95;
+    
+    //UART.printf("%d %d %d\r\n", last_result, xzy[0], xzy[2]);  // should not be printing in interrupt, am going to hell
+    pc.printf("%f, %f, %f\r\n", RMS, low_pass, decay);  // should not be printing in interrupt, am going to hell
+    if(counter > 15){
+        last_result = 0;
+        counter = 0;
+    }
+    if(last_result)counter++;
+}
+/*
+void spew_accel(){
+    static float low_pass1 = 0.0;
+    static float low_pass2 = 0.0;
+    float RMS = 0.0;
+    static float last_decay = 0.0;
+    float decay = 0.0;
+    static float accumulator = 0.0;
+    bool accumulating = false;
+    float jerk = 0.0;
+    //static float biggest_jerk = 0.0;
+    
+    //static int16_t xzy[3];
+    LIS3DH_readAll(xzy);
+    //RMS = (float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2];
+    float t1 = xzy[0];
+    float t2 = xzy[2];
+    RMS = sqrt(t1*t1 + t2*t2);
+    //RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
+    jerk = low_pass1;
+    low_pass1 = 0.35*RMS + 0.65*low_pass1;
+    low_pass2 = 0.025*RMS + 0.975*low_pass2;
+
+    jerk = low_pass1 - jerk;
+    if(jerk > 500)accumulating = true;
+    //if(jerk > biggest_jerk)biggest_jerk = jerk;
+    
+    //if(RMS > decay)decay = RMS;
+    //decay = decay * 0.9999;
+    decay = low_pass1 - low_pass2;
+    if(decay < 0) decay = 0;
+
+    if(decay < 400){
+      if(accumulator > 10000)UART.printf("s%ld\r", (long)accumulator/30);
+      accumulator = 0;
+      accumulating = false;
+      //biggest_jerk = 0;
+    }
+    else if(accumulating)accumulator += decay;
+    last_decay = decay;
+    
+}*/
+
+void getRadioInput(char *data, uint16_t length)
+{
+  static int i = 0;
+  static char parameter = '_';
+  static char buffer[RADIO_MESSAGE_LEN + 1];
+  int value = 0;
+  
+  // listen for commands coming over bluetooth
+  for (int j = 0; j < length; ++j){
+    char ch = data[j];
+
+    pc.printf("looping: %c. %c\r\n", ch, parameter);
+
+    // if ch is a message terminator, if parameter is anything other than '_', process the message
+    if((ch == '\r' || ch == ';' || ch == '\n'))
+    {
+      if(i > 0  && parameter != '_')
+      {
+        buffer[i-1] = 0;
+        value = atoi(buffer);
+        
+        interpret(parameter, value);
+      }
+
+      //  done sending, reset parameters
+      parameter = '_';
+      buffer[0] = 0;
+      i=0;
+    }
+    else
+    { // got anything but a message terminator
+      if(i==0)
+        parameter = ch; // nothing accumulated yet
+      else 
+        buffer[i-1] = ch;
+        
+      i++;
+    }
+    //UART.printf("%c: %s, %d\r\n",parameter, buffer, i);
+  }
+}
+
+void interpret(char parameter, int value){
+    
+    switch(parameter){
+        case 'g':
+            if(value == 1){
+                new_strike = false;
+                //active = true;
+                if(mode == STRIKE_DETECT_1)measure.attach_us(&strike_detect1, timestep*1000);
+                if(mode == STRIKE_DETECT_2)measure.attach_us(&strike_detect2, timestep*1000);
+            }
+            if(value == 0)measure.detach(); //active = false;
+        break;
+        case 'm':
+            mode = value;
+        break;
+        case 'p':
+            config_parameter = value;
+            //UART.printf("parameter is: %d\r\n", value);
+        break;
+        case 'v':
+            if(config_parameter < 8)set_setting(config_parameter, value);
+            else{
+                //UART.printf("value is: %d\r\n", value);
+                if(config_parameter == 101){
+                    //fast_filter = value;
+                    c1_1 = (float)value/1000.0;
+                    c2_1 = 1.0 - c1_1;
+                }
+                if(config_parameter == 102){
+                    //slow_filter = value;
+                    c1_2 = (float)value/1000.0;
+                    c2_2 = 1.0 - c1_2;
+                }
+                if(config_parameter == 103)big_hit_threshold = value;
+                if(config_parameter == 104)small_hit_threshold = value;
+                if(config_parameter == 105)jerk_threshold = value;
+                if(config_parameter == 106){
+                    float x = value;
+                    shadow_decay = 0.9 + x*0.0001;
+                    //UART.printf("shadow_decay is: %f\r\n", shadow_decay);
+                }
+            }
+        break;
+        case 's':
+            if(value > 0){
+                //measure.attach_us(&spew_accel, 1000);
+                //spew.attach(&spew_accel, float(value)/1000);
+                //measure.attach_us(&read_accel, 1000);
+            }
+            if(value == 0)spew.detach(); //active = false;
+        break;
+        case 't':
+            //y(k) = a * y(k-1) + (1-a) * x(k)
+            //a = exp (-T/tau)
+            break;
+        case 'z':
+            pc.printf(VERSION);
+        break;       
+        default:
+        // print stuff
+        break;
     }
 }