Diff: main.cpp

Revision:

13:da8097acb518

Parent:

11:4a99e73b88cc

--- a/main.cpp	Sun May 21 13:01:32 2017 +0000
+++ b/main.cpp	Tue Aug 08 16:32:14 2017 +0000
@@ -36,12 +36,30 @@
  ******************************************************************************
 */
 
+/**************************************************************
+*************** THIS HANDLES HOPPER WINDOW ********************
+***************************************************************/
+
 /* Includes */
 #include "mbed.h"
 #include "x_nucleo_iks01a1.h"
 
 // define sample frequency
-#define SAMPLES_DELAY 5000
+#define SAMPLES_DELAY 5000.f
+#define ARR_LEN 100
+#define MG2MM 100.f
+#define GYRO_FACTOR 1000.f
+
+// thresholds
+#define EVENT_THRESHOLD 1500.f
+#define MOVEMENT_THRESHOLD 5.f
+#define VEL_THRESHOLD 15.f
+#define STILL_THRESHOLD 2.f
+#define OPEN_THRESHOLD 5.f
+#define CLOSE_THRESHOLD 5.f
+
+#define PI 3.14159265f
+
 /* Instantiate the expansion board */
 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
 
@@ -60,26 +78,156 @@
 
 // send data
 bool flagData=0;
+float sum_v = 0, sum_s = 0;
+float gyro_bias[3], acc_bias = 0;
+int axis = 0;
 
 // time stamp
 int timeStamp_us=0;
 int sampleStamp=0;
 
+
+DigitalOut led1(LED1);
+
+/*
+float pitch(float x, float y, float z) {
+    float calc, angle;
+    calc = pow(x, 2) / sqrt(pow(y, 2) + pow(z, 2));
+    angle = atan(calc) * 180 / PI;
+    return angle;
+}
+
+
+float roll(float x, float y, float z) {
+    float calc, angle;
+    calc = pow(y, 2) / sqrt(pow(x, 2) + pow(z, 2));
+    angle = atan(calc) * 180 / PI;
+    return angle;
+}
+*/
+
+float lowpass(float sample, float prev_lp) {
+    float lp_factor = 0.005;
+    float new_lp;
+    new_lp = sample * lp_factor + prev_lp * (1 - lp_factor);
+    return new_lp;
+}
+
+
+float highpass(float sample, float prev_sample, float prev_hp) {
+    float hp_factor = 0.9;
+    float new_hp;
+    new_hp = prev_hp * hp_factor + (sample - prev_sample) * hp_factor;
+    return new_hp;
+}
+
+
+float integrate(float sample, float prev_sum) {
+    float ret;
+    ret = prev_sum + sample;
+    return ret;
+}
+
+/*
+bool check_event_start(float* samples, int i) {
+    sum_v = 0;
+    sum_s = 0;
+    for (int j=1; j <= ARR_LEN; j++){
+        sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], sum_v);
+        float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
+        sum_s = integrate(effective_vel, sum_s);
+    }
+    sum_s = sum_s / 1000;
+    if (abs(sum_s) > MOVEMENT_THRESHOLD) {
+        return true;
+    }
+    return false;
+}*/
+
+
+/*bool check_event_end(float* samples, int i) {
+    float temp_sum_v = 0, temp_sum_s = 0;
+    for (uint8_t j=1; j <= ARR_LEN; j++){
+        temp_sum_v = integrate(samples[ (i + j) % ARR_LEN] - bias[axis], temp_sum_v);
+        temp_sum_s = integrate(temp_sum_v, temp_sum_s);
+    }
+    
+    if (temp_sum_s < STILL_THRESHOLD) {
+        return false;
+    }
+    return true;
+}*/
+
 /* Simple main function */
 int main()
 {
     pc.baud(921600);
     timer.start();
     timeStamp_us = timer.read_us();
+    int i = 0, j;
     uint8_t id;
     int32_t Acc_axes[3];
     int32_t Gyro_axes[3];
-
+    //float diff, prev_lp = 0, prev_hp = 0;
+    float acc_samp[3][ARR_LEN] = {}, acc_lp[3][ARR_LEN] = {};
+    float gyro_samp[3][ARR_LEN] = {}, gyro_rad[3][ARR_LEN] = {}, gyro_hp[3][ARR_LEN] = {};
+    float complementary[3][ARR_LEN] = {};
+    //float pitch_calc[ARR_LEN], roll_calc[ARR_LEN];
+    bool opened = false;
+    
+    // get init data and bias
+    for(j=0; j < ARR_LEN; j++){
+        accelerometer->get_x_axes(Acc_axes);
+        gyroscope->get_g_axes(Gyro_axes);
+        //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
+        
+        // get accelerometer starting data
+        acc_samp[0][j % ARR_LEN] = Acc_axes[0];
+        acc_samp[1][j % ARR_LEN] = Acc_axes[1];
+        acc_samp[2][j % ARR_LEN] = Acc_axes[2];
+        acc_bias += (acc_samp[1][j % ARR_LEN] / ARR_LEN);
+        
+        // get gyroscope starting data
+        gyro_samp[0][j % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
+        gyro_samp[1][j % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
+        gyro_samp[2][j % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
+        gyro_bias[0] += (gyro_samp[0][j % ARR_LEN] / ARR_LEN);
+        gyro_bias[1] += (gyro_samp[1][j % ARR_LEN] / ARR_LEN);
+        gyro_bias[2] += (gyro_samp[2][j % ARR_LEN] / ARR_LEN);
+    }
+    
+    // get init lowpass on acc, integral + highpass on gyro and complementary
+    for(j=0; j < ARR_LEN; j++){ 
+        // get acc lowpass data
+        acc_lp[0][j % ARR_LEN] = lowpass(acc_samp[0][j % ARR_LEN], acc_lp[0][(j-1) % ARR_LEN]);
+        acc_lp[1][j % ARR_LEN] = lowpass(acc_samp[1][j % ARR_LEN], acc_lp[1][(j-1) % ARR_LEN]);
+        acc_lp[2][j % ARR_LEN] = lowpass(acc_samp[2][j % ARR_LEN], acc_lp[2][(j-1) % ARR_LEN]);
+        
+        // get gyro highpass data
+        gyro_rad[0][j % ARR_LEN] = integrate(gyro_samp[0][j % ARR_LEN] - gyro_bias[0], gyro_rad[0][(j-1) % ARR_LEN]);
+        gyro_rad[1][j % ARR_LEN] = integrate(gyro_samp[1][j % ARR_LEN] - gyro_bias[1], gyro_rad[1][(j-1) % ARR_LEN]);
+        gyro_rad[2][j % ARR_LEN] = integrate(gyro_samp[2][j % ARR_LEN] - gyro_bias[2], gyro_rad[2][(j-1) % ARR_LEN]);
+        gyro_hp[0][j % ARR_LEN] = highpass(gyro_rad[0][j % ARR_LEN], gyro_rad[0][(j-1) % ARR_LEN], gyro_hp[0][(j-1) % ARR_LEN]);
+        gyro_hp[1][j % ARR_LEN] = highpass(gyro_rad[1][j % ARR_LEN], gyro_rad[1][(j-1) % ARR_LEN], gyro_hp[1][(j-1) % ARR_LEN]);
+        gyro_hp[2][j % ARR_LEN] = highpass(gyro_rad[2][j % ARR_LEN], gyro_rad[2][(j-1) % ARR_LEN], gyro_hp[2][(j-1) % ARR_LEN]);
+        
+        // combine to complementary
+        complementary[0][j % ARR_LEN] = acc_lp[0][j % ARR_LEN] + gyro_hp[0][j % ARR_LEN];
+        complementary[1][j % ARR_LEN] = acc_lp[1][j % ARR_LEN] + gyro_hp[1][j % ARR_LEN];
+        complementary[2][j % ARR_LEN] = acc_lp[2][j % ARR_LEN] + gyro_hp[2][j % ARR_LEN];
+        
+        // get pitch and roll
+        //pitch_calc[j % ARR_LEN] = pitch(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
+        //roll_calc[j % ARR_LEN] = roll(complementary[0][j % ARR_LEN], complementary[1][j % ARR_LEN], complementary[2][j % ARR_LEN]);
+        //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[j % ARR_LEN], roll_calc[j % ARR_LEN]);
+    }
+    
     pc.printf("\r\n--- Starting new run ---\r\n");
 
     gyroscope->read_id(&id);
     pc.printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
-
+    pc.printf("bias for gyro: %f, %f, %f\r\n", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
+    
     wait(0.5);
 
     while(1) {
@@ -91,11 +239,95 @@
             // get raw data
             accelerometer->get_x_axes(Acc_axes);
             gyroscope->get_g_axes(Gyro_axes);
-            pc.printf("D:%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
-            //pc.printf("D:%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2]);
-            // stability delay to finish sending data
-            //wait_us(10);
+            //pc.printf("%7ld,%7ld,%7ld,%7ld,%7ld,%7ld,%7ld\r\n", sampleStamp, Acc_axes[0], Acc_axes[1], Acc_axes[2], Gyro_axes[0], Gyro_axes[1], Gyro_axes[2]);
+            
+            // update acc data
+            acc_samp[0][i % ARR_LEN] = Acc_axes[0];
+            acc_samp[1][i % ARR_LEN] = Acc_axes[1];
+            acc_samp[2][i % ARR_LEN] = Acc_axes[2];
+            // update gyro data
+            gyro_samp[0][i % ARR_LEN] = Gyro_axes[0] / GYRO_FACTOR;
+            gyro_samp[1][i % ARR_LEN] = Gyro_axes[1] / GYRO_FACTOR;
+            gyro_samp[2][i % ARR_LEN] = Gyro_axes[2] / GYRO_FACTOR;
+            
+            // get acc lowpass data
+            acc_lp[0][i % ARR_LEN] = lowpass(acc_samp[0][i % ARR_LEN], acc_lp[0][(i-1) % ARR_LEN]);
+            acc_lp[1][i % ARR_LEN] = lowpass(acc_samp[1][i % ARR_LEN], acc_lp[1][(i-1) % ARR_LEN]);
+            acc_lp[2][i % ARR_LEN] = lowpass(acc_samp[2][i % ARR_LEN], acc_lp[2][(i-1) % ARR_LEN]);
+            
+            // get gyro highpass data
+            gyro_rad[0][i % ARR_LEN] = integrate(gyro_samp[0][i % ARR_LEN] - gyro_bias[0], gyro_rad[0][(i-1) % ARR_LEN]);
+            gyro_rad[1][i % ARR_LEN] = integrate(gyro_samp[1][i % ARR_LEN] - gyro_bias[1], gyro_rad[1][(i-1) % ARR_LEN]);
+            gyro_rad[2][i % ARR_LEN] = integrate(gyro_samp[2][i % ARR_LEN] - gyro_bias[2], gyro_rad[2][(i-1) % ARR_LEN]);
+            gyro_hp[0][i % ARR_LEN] = highpass(gyro_rad[0][i % ARR_LEN], gyro_rad[0][(j-1) % ARR_LEN], gyro_hp[0][(i-1) % ARR_LEN]);
+            gyro_hp[1][i % ARR_LEN] = highpass(gyro_rad[1][i % ARR_LEN], gyro_rad[1][(j-1) % ARR_LEN], gyro_hp[1][(i-1) % ARR_LEN]);
+            gyro_hp[2][i % ARR_LEN] = highpass(gyro_rad[2][i % ARR_LEN], gyro_rad[2][(j-1) % ARR_LEN], gyro_hp[2][(i-1) % ARR_LEN]);
+            
+            //pc.printf("%f acc0, %f acc1, %f acc2\r\n", acc_lp[0][i % ARR_LEN], acc_lp[1][i % ARR_LEN], acc_lp[2][i % ARR_LEN]);
+            
+            
+            // combine to complementary
+            complementary[0][i % ARR_LEN] = acc_lp[0][i % ARR_LEN] + gyro_hp[0][i % ARR_LEN];
+            complementary[1][i % ARR_LEN] = acc_lp[1][i % ARR_LEN] + gyro_hp[1][i % ARR_LEN];
+            complementary[2][i % ARR_LEN] = acc_lp[2][i % ARR_LEN] + gyro_hp[2][i % ARR_LEN];
+            
+            //pc.printf("%f acc0, %f compl0\r\n" ,acc_lp[0][i % ARR_LEN], complementary[0][i % ARR_LEN]); // gryo_rad[0]
+            
+            // get pitch and roll
+            //pitch_calc[i % ARR_LEN] = pitch(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
+            //roll_calc[i % ARR_LEN] = roll(complementary[0][i % ARR_LEN], complementary[1][i % ARR_LEN], complementary[2][i % ARR_LEN]);
+            
+            //pc.printf("pitch: %f\t roll: %f\r\n", pitch_calc[i % ARR_LEN], roll_calc[i % ARR_LEN]);
+            
+            //diff = abs(samples[axis][i % ARR_LEN] - samples[axis][(i - (ARR_LEN / 2)) % ARR_LEN]);
+            // pc.printf("%f,%f,%f\t%f\r\n", samples[0][i % ARR_LEN], samples[1][i % ARR_LEN], samples[2][i % ARR_LEN], diff);
+            if (abs(complementary[0][i % ARR_LEN]) > EVENT_THRESHOLD) {
+                if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) > 35 ){
+                    opened = true;
+                    led1 =true;
+                }
+                else if (abs(acc_samp[1][i % ARR_LEN] - acc_bias) < 15){
+                    opened = false;
+                    led1 = false;
+                }
+            }
+            /*if(opened){
+                pc.printf("1, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
+            }
+            else {
+                pc.printf("0, %f\r\n", abs(acc_samp[1][i % ARR_LEN] - acc_bias));
+            }*/
+            i++;
+                /*//pc.printf("suspected event: ");
+                event_flag = check_event_start(samples[axis], i);
+                if(event_flag) {
+                    pc.printf("OPENED: %f\r\n", sum_s);
+                }
+                else {
+                    pc.printf("False sum_s: %f\r\n", sum_s);
+                }
+                //event_flag = false 
+            }
+            else if (event_flag){
+                sum_v = integrate(samples[axis][i % ARR_LEN] - bias[axis], sum_v);
+                //float effective_vel = abs(sum_v) >= VEL_THRESHOLD ? sum_v : 0;
+                sum_v = abs(sum_v) < VEL_THRESHOLD ? 0 : sum_v;
+                sum_s = integrate(sum_v, sum_s);
+                pc.printf("sum_v: %f, sum_s: %f\r\n", sum_v, sum_s);
+                if (abs(sum_s) <= CLOSE_THRESHOLD){
+                    pc.printf("CLOSED: %f\r\n", sum_s);
+                    event_flag = false;
+                    sum_s = 0;
+                    sum_v = 0;
+                }
+            }*/
         }// end get & send samples
-
     }// end loop
 }// end main
+
+
+
+//TODO list:
+// compute event movement
+// find event ending
+// display 0,1
\ No newline at end of file