Revision 7:bade9a130c05, committed 2015-07-23

Comitter:

oaa36

Date:

Thu Jul 23 14:07:41 2015 +0000

Parent:

6:6710cd90a81c

Commit message:

more changes;

Changed in this revision

--- a/main.cpp	Thu Jul 23 11:00:23 2015 +0000
+++ b/main.cpp	Thu Jul 23 14:07:41 2015 +0000
@@ -3,7 +3,6 @@
 #include "MMA8451Q.h"
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
-#define TRANSFER_SIZE 24
 
 Serial pc(USBTX, USBRX); // tx, rx
 
@@ -15,32 +14,36 @@
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 
-#include <vector>
+#include <deque>
 #include <math.h>
 #include <iostream>
 
 
-int main() {
+int main()
+{
+
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
 
+#define TRANSFER_SIZE   24
+
     char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
     int txDataCnt = 0;
     int rxDataCnt = 0;
 
-    std::vector<int> x_data;
-    std::vector<int> y_data;
-    std::vector<int> z_data;
-    //std::vector<int> z_sig;
-    //std::vector<int> y_sig;
-    //std::vector<int> x_sig;
+    std::deque<float> x_data;
+    std::deque<float> y_data;
+    std::deque<float> z_data;
+    //std::deque<float> z_sig;
+    //std::deque<float> y_sig;
+    //std::deque<float> x_sig;
 
-    std::vector<int> thetavec_x;
-    std::vector<int> thetavec_av_x;
-    std::vector<int> thetavec_y;
-    std::vector<int> thetavec_av_y;
-    std::vector<int> thetavec_z;
-    std::vector<int> thetavec_av_z;
-    std::vector<int> sig;
+    std::deque<float> thetavec_x;
+    std::deque<float> thetavec_av_x;
+    std::deque<float> thetavec_y;
+    std::deque<float> thetavec_av_y;
+    std::deque<float> thetavec_z;
+    std::deque<float> thetavec_av_z;
+    std::deque<float> sig;
 
     x_data.assign (20,0);
     y_data.assign (20,0);
@@ -56,20 +59,23 @@
     thetavec_av_z.assign (20, 0);
     sig.assign (20,0);
 
-    int threshold1 = 1.0105;
-    int threshold2 = 1.2639;
-    int threshold3 = 0.8398;
-    int threshold4 = 0.2050;
-    int threshold5 = 0.5905;
+    const float threshold1 = 1.0105;
+    const float threshold2 = 1.2639;
+    const float threshold3 = 0.8398;
+    const float threshold4 = 0.2050;
+    const float threshold5 = 0.5905;
 
     /*my_nrf24l01p.powerUp();
     my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
     my_nrf24l01p.setReceiveMode();
     my_nrf24l01p.enable();*/
 
+
+
     printf("MMA8451 ID: %d\n", acc.getWhoAmI());
 
     while (1) {
+
         float x, y, z, sum, root;
         float average_x, average_y, average_z;
         float variance_x, variance_y, variance_z;
@@ -96,6 +102,9 @@
         float sum_sig1, sum_sig;
         float sigA, sigA1;
 
+
+
+
         x = abs(acc.getAccX());
         //printf("%f", x);
         y = abs(acc.getAccY());
@@ -104,39 +113,48 @@
         root = sqrt(sum);
         //printf("%f", root);
 
-        theta_z = acos(z/root);
+        theta_z = acos((z / root));
         //theta_y = acos(y/root);
         //theta_x = acos(x/root);
 
-        x_data.push_back (x);
-        y_data.push_back (y);
-        z_data.push_back (z);
-        sig.push_back (root);
+        x_data.push_front (x);
+        y_data.push_front (y);
+        z_data.push_front (z);
+        sig.push_front (root);
 
         x_data.pop_back();
         y_data.pop_back();
         z_data.pop_back();
         sig.pop_back ();
 
+
         //printf ("x_data\n\r");
+
         //for (int p = 0; p != x_data.size(); ++p)
         //{
              //cout << x_data[p] << "\n\r" << endl;
              //cout << x_data.at(p) << "\n\r" << endl;
         //}
 
-        thetavec_z.push_back (theta_z);
-        //thetavec_y.push_back (theta_y);
-        //thetavec_x.push_back (theta_x);
+        thetavec_z.push_front (theta_z);
+        //thetavec_y.push_front (theta_y);
+        //thetavec_x.push_front (theta_x);
 
         thetavec_z.pop_back ();
         //thetavec_y.pop_back ();
         //thetavec_x.pop_back ();
 
+
+
         int  i, n = 20, m = 10;
+        //
+        for (i = 0; i < m; i++)
+
 
         /*  Compute  mean  */
-        for (i = 0; i < m; i++) {
+
+        for (i = 0; i < m; i++)
+        {
             total_x = total_x + x_data[i];
             total_y = total_y + y_data[i];
             total_z = total_z + z_data[i];
@@ -148,11 +166,13 @@
         average_z = total_z / (float)m;
         average_sig = total_sig / (float)m;
 
-        for (i = 10; i < n; i++) {
+         for (i = 10; i < n; i++)
+        {
             total_x1 = total_x1 + x_data[i];
             total_y1 = total_y1 + y_data[i];
             total_z1 = total_z1 + z_data[i];
             total_sig1 = total_sig1 + sig[i];
+
         }
 
         average_x1 = total_x1 / (float)m;
@@ -162,7 +182,8 @@
 
 
         /*  Compute  variance  and standard deviation  */
-        for (i = 0; i < m; i++) {
+        for (i = 0; i < m; i++)
+        {
             sum1_x = sum1_x + pow((x_data[i] - average_x), 2);
             sum1_y = sum1_y + pow((y_data[i] - average_y), 2);
             sum1_z = sum1_z + pow((z_data[i] - average_z), 2);
@@ -178,7 +199,8 @@
         std_deviation_z = sqrt(variance_z);
         sigA = sqrt(variance_sig);
 
-        for (i = 10; i < n; i++) {
+         for (i = 10; i < n; i++)
+        {
             sum1_x1 = sum1_x1 + pow((x_data[i] - average_x1), 2);
             sum1_y1 = sum1_y1 + pow((y_data[i] - average_y1), 2);
             sum1_z1 = sum1_z1 + pow((z_data[i] - average_z1), 2);
@@ -193,7 +215,7 @@
         std_deviation_x1 = sqrt(variance_x1);
         std_deviation_y1 = sqrt(variance_y1);
         std_deviation_z1 = sqrt(variance_z1);
-        sigA1 = sqrt(variance_sig);
+        sigA1 = sqrt(variance_sig1);
 
         sqr_sum_xyz = pow(std_deviation_x, 2) + pow(std_deviation_y, 2) + pow(std_deviation_z, 2);
         mag_sigXYZ = sqrt(sqr_sum_xyz);
@@ -202,20 +224,21 @@
 
 
 
-        for (i = 0; i < m; i++) {
+        for (i = 0; i < m; i++)
+        {
             total_theta_z1 = total_theta_z1 + thetavec_z[i];
             //total_theta_y1 = total_theta_y1 + thetavec_y[i];
             //total_theta_x1 = total_theta_x1 + thetavec_x[i];
         }
 
-        theta_average_z = total_theta_z1 / (float)m;
-        //theta_average_y = total_theta_y1 / (float)m;
-        //theta_average_x = total_theta_x1 / (float)m;
+         theta_average_z = total_theta_z1 / (float)m;
+         //theta_average_y = total_theta_y1 / (float)m;
+         //theta_average_x = total_theta_x1 / (float)m;
 
 
-        thetavec_av_z.push_back(theta_average_z);
-        //thetavec_av_y.push_back(theta_average_y);
-        //thetavec_av_x.push_back(theta_average_x);
+        thetavec_av_z.push_front(theta_average_z);
+        //thetavec_av_y.push_front(theta_average_y);
+        //thetavec_av_x.push_front(theta_average_x);
 
 
         thetavec_av_z.pop_back();
@@ -233,19 +256,26 @@
 
         float ON;
 
-        if ((sig_ratio > threshold1) || (theta_ratio_z > threshold2)) {
-            //have to add pressure sensor;
-            if (((theta_ratio_z > threshold3) || (dtheta_z > threshold4)) && ((sigA > threshold4) || (mag_sigXYZ > threshold5))) {
-                printf ("fall\n\r");
+        if ((sig_ratio > threshold1) || (theta_ratio_z > threshold2))
+            {//have to add pressure sensor;
+            if (((theta_ratio_z > threshold3) || (dtheta_z > threshold4)) && ((sigA > threshold4) || (mag_sigXYZ > threshold5)))
+                {printf ("fall\n\r");
                 ON = 1;
-            } else {
-                printf ("no fall\n\r");
+                }
+            else
+                {printf ("no fall\n\r");
                 ON = 0;
+                }
+                }
+        else
+            {printf ("no fall\n\r");
+            ON = 0;
             }
-        } else {
-            printf ("no fall\n\r");
-            ON = 0;
-        }
+
+
+
+
+
 
         //printf("X:%f,Y:%f,Z:%f\n", x, y, z);
 
@@ -255,4 +285,4 @@
 
         wait(0.05);
     }
-}
+}
\ No newline at end of file