Revision 5:9195756445e4, committed 2015-07-23

Comitter:

oaa36

Date:

Thu Jul 23 10:51:17 2015 +0000

Parent:

4:7d3a1dfe5454

Child:

6:6710cd90a81c

Child:

8:81a97ad26339

Commit message:

Pre-deque

Changed in this revision

--- a/main.cpp	Mon Jun 29 12:29:05 2015 +0000
+++ b/main.cpp	Thu Jul 23 10:51:17 2015 +0000
@@ -1,83 +1,288 @@
 #include "mbed.h"
 #include "nRF24L01P.h"
 #include "MMA8451Q.h"
- 
+
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
- 
+
 Serial pc(USBTX, USBRX); // tx, rx
- 
+
 PinName const SDA = PTE25;
 PinName const SCL = PTE24;
- 
+
 nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTE1, PTE0, PTD0);    // mosi, miso, sck, csn, ce, irq
- 
+
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
- 
+
+#include <vector>
+#include <math.h>
+#include <iostream>
+
+
 int main()
 {
- 
+
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    //PwmOut rled(LED1);
-    //PwmOut gled(LED2);
-    //PwmOut bled(LED3);
- 
-// The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
-//  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
-//  only handles 4 byte transfers in the ATMega code.
+
 #define TRANSFER_SIZE   24
- 
+
     char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
     int txDataCnt = 0;
     int rxDataCnt = 0;
- 
-    my_nrf24l01p.powerUp();
- 
-    // Display the (default) setup of the nRF24L01+ chip
-    //pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
-    //pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
-    //pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
-    //pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
-    //pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
- 
-    //pc.printf( "Type keys to test transfers:\r\n  (transfers are grouped into %d characters)\r\n", TRANSFER_SIZE );
- 
+
+    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::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;
+
+    x_data.assign (20,0);
+    y_data.assign (20,0);
+    z_data.assign (20,0);
+    //x_sig.assign (20,0);
+    //y_sig.assign (20,0);
+    //z_sig.assign (20,0);
+    thetavec_x.assign (20, 0);
+    thetavec_av_x.assign (20, 0);
+    thetavec_y.assign (20, 0);
+    thetavec_av_y.assign (20, 0);
+    thetavec_z.assign (20, 0);
+    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;
+
+    /*my_nrf24l01p.powerUp();
     my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
- 
     my_nrf24l01p.setReceiveMode();
-    my_nrf24l01p.enable();
- 
- 
- 
+    my_nrf24l01p.enable();*/
+
+
+
     printf("MMA8451 ID: %d\n", acc.getWhoAmI());
- 
+
     while (1) {
- 
-        float x, y, z;
+
+        float x, y, z, sum, root;
+        float average_x, average_y, average_z;
+        float variance_x, variance_y, variance_z;
+        float std_deviation_x, std_deviation_y, std_deviation_z;
+        float total_x = 0, total_y = 0, total_z = 0;
+        float sum1_x = 0, sum1_y = 0, sum1_z = 0;
+        float average_x1, average_y1, average_z1;
+        float variance_x1, variance_y1, variance_z1;
+        float std_deviation_x1, std_deviation_y1, std_deviation_z1;
+        float total_x1 = 0, total_y1 = 0, total_z1 = 0;
+        float sum1_x1 = 0, sum1_y1 = 0, sum1_z1 = 0;
+        float sqr_sum_xyz, mag_sigXYZ;
+        float theta_average_z, theta_average_y, theta_average_x;
+        float theta_ratio_x, theta_ratio_y, theta_ratio_z;
+        float dtheta_z, dtheta_y, dtheta_x;
+        float sig_x, sig_y, sig_z;
+        float theta_z, theta_y, theta_x;
+        float sigratio_z, sigratio_y, sigratio_x;
+        float total_theta_z1, total_theta_y1, total_theta_x1;
+        float sig_ratio;
+        float total_sig, total_sig1;
+        float average_sig, average_sig1;
+        float variance_sig1, variance_sig;
+        float sum_sig1, sum_sig;
+        float sigA, sigA1;
+
+
+
+
         x = abs(acc.getAccX());
+        //printf("%f", x);
         y = abs(acc.getAccY());
         z = abs(acc.getAccZ());
-        //rled = 1.0f - x;
-        //gled = 1.0f - y;
-        //bled = 1.0f - z;
-        //wait(0.01);
-        //txDataCnt =  sprintf(txData, "X:%1.3f,Y:%1.3f,Z:%1.3f\n", x,y,z);
-         txDataCnt =  sprintf(txData, "  %1.3f   %1.3f   %1.3f\n", x,y,z);
-        printf("X:%f,Y:%f,Z:%f size %i %s\n", x, y, z,txDataCnt, txData);
- 
-        // If we've received anything over the host serial link...
- 
-        // ...add it to the transmit buffer
-        //txData[txDataCnt++] = pc.getc();
- 
-        // If the transmit buffer is full
- 
-        // Send the transmitbuffer via the nRF24L01+
-        my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt );
-        
-        printf("sent");
- 
-        // Toggle LED1 (to help debug Host -> nRF24L01+ communication)
-        myled1 = !myled1;
+        sum = pow(x, 2)+ pow(y, 2) + pow(z, 2);
+        root = sqrt(sum);
+        //printf("%f", 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.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.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++)
+        {
+            total_x = total_x + x_data[i];
+            total_y = total_y + y_data[i];
+            total_z = total_z + z_data[i];
+            total_sig = total_sig + sig [i];
+        }
+
+        average_x = total_x / (float)m;
+        average_y = total_y / (float)m;
+        average_z = total_z / (float)m;
+        average_sig = total_sig / (float)m;
+
+         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;
+        average_y1 = total_y1 / (float)m;
+        average_z1 = total_z1 / (float)m;
+        average_sig1 = total_sig1 / (float)m;
+
+
+        /*  Compute  variance  and standard deviation  */
+        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);
+            sum_sig = sum_sig + pow((sig[i] - average_sig), 2);
+        }
+        variance_x = sum1_x / (float)m;
+        variance_y = sum1_y / (float)m;
+        variance_z = sum1_z / (float)m;
+        variance_sig = sum_sig / (float)m;
+
+        std_deviation_x = sqrt(variance_x);
+        std_deviation_y = sqrt(variance_y);
+        std_deviation_z = sqrt(variance_z);
+        sigA = sqrt(variance_sig);
+
+         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);
+            sum_sig1 = sum_sig1 + pow((sig[i] - average_sig1), 2);
+        }
+        variance_x1 = sum1_x1 / (float)m;
+        variance_y1 = sum1_y1 / (float)m;
+        variance_z1 = sum1_z1 / (float)m;
+        variance_sig1 = sum_sig1 / (float)m;
+
+
+        std_deviation_x1 = sqrt(variance_x1);
+        std_deviation_y1 = sqrt(variance_y1);
+        std_deviation_z1 = sqrt(variance_z1);
+        sigA1 = sqrt(variance_sig);
+
+        sqr_sum_xyz = pow(std_deviation_x, 2) + pow(std_deviation_y, 2) + pow(std_deviation_z, 2);
+        mag_sigXYZ = sqrt(sqr_sum_xyz);
+
+        sig_ratio = sigA / sigA1;
+
+
+
+        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;
+
+
+        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.pop_back();
+        //thetavec_av_y.pop_back();
+        //thetavec_av_x.pop_back();
+
+        theta_ratio_z = thetavec_av_z[0]/thetavec_av_z[10];
+        //theta_ratio_y = thetavec_av_y[0]/thetavec_av_y[10];
+        //theta_ratio_x = thetavec_av_x[0]/thetavec_av_x[10];
+
+        dtheta_z = thetavec_av_z[0]-thetavec_av_z[10];
+        // dtheta_y = thetavec_av_y[0]-thetavec_av_y[10];
+        // dtheta_x = thetavec_av_x[0]-thetavec_av_x[10];
+
+
+        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");
+                ON = 1;
+                }
+            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);
+
+        //my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt );
+
+        //printf("sent");
+
+        wait(0.05);
     }
 }
\ No newline at end of file