Sensor CDT / Mbed 2 deprecated Omar_Plan_to_save_the_world

For Josie

Dependencies:   MMA8451Q mbed nRF24L01P

Fork of Acclerometer_node by Sensor CDT

main.cpp@6:6710cd90a81c, 2015-07-23 (annotated)

Committer:
oaa36
Date:
Thu Jul 23 11:00:23 2015 +0000
Revision:
6:6710cd90a81c
Parent:
5:9195756445e4
Child:
7:bade9a130c05
James's Edit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Owen 0:a51a6e7da590 1 #include "mbed.h"
Owen 0:a51a6e7da590 2 #include "nRF24L01P.h"
trisjph 3:5c7f4e5a7605 3 #include "MMA8451Q.h"
oaa36 5:9195756445e4 4
trisjph 3:5c7f4e5a7605 5 #define MMA8451_I2C_ADDRESS (0x1d<<1)
oaa36 6:6710cd90a81c 6 #define TRANSFER_SIZE 24
oaa36 5:9195756445e4 7
Owen 0:a51a6e7da590 8 Serial pc(USBTX, USBRX); // tx, rx
oaa36 5:9195756445e4 9
trisjph 3:5c7f4e5a7605 10 PinName const SDA = PTE25;
trisjph 3:5c7f4e5a7605 11 PinName const SCL = PTE24;
oaa36 5:9195756445e4 12
jaehughes 4:7d3a1dfe5454 13 nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTE1, PTE0, PTD0); // mosi, miso, sck, csn, ce, irq
oaa36 5:9195756445e4 14
Owen 0:a51a6e7da590 15 DigitalOut myled1(LED1);
Owen 0:a51a6e7da590 16 DigitalOut myled2(LED2);
oaa36 5:9195756445e4 17
oaa36 5:9195756445e4 18 #include <vector>
oaa36 5:9195756445e4 19 #include <math.h>
oaa36 5:9195756445e4 20 #include <iostream>
oaa36 5:9195756445e4 21
oaa36 5:9195756445e4 22
oaa36 6:6710cd90a81c 23 int main() {
trisjph 3:5c7f4e5a7605 24 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
oaa36 5:9195756445e4 25
Owen 0:a51a6e7da590 26 char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
Owen 0:a51a6e7da590 27 int txDataCnt = 0;
Owen 0:a51a6e7da590 28 int rxDataCnt = 0;
oaa36 5:9195756445e4 29
oaa36 5:9195756445e4 30 std::vector<int> x_data;
oaa36 5:9195756445e4 31 std::vector<int> y_data;
oaa36 5:9195756445e4 32 std::vector<int> z_data;
oaa36 5:9195756445e4 33 //std::vector<int> z_sig;
oaa36 5:9195756445e4 34 //std::vector<int> y_sig;
oaa36 5:9195756445e4 35 //std::vector<int> x_sig;
oaa36 5:9195756445e4 36
oaa36 5:9195756445e4 37 std::vector<int> thetavec_x;
oaa36 5:9195756445e4 38 std::vector<int> thetavec_av_x;
oaa36 5:9195756445e4 39 std::vector<int> thetavec_y;
oaa36 5:9195756445e4 40 std::vector<int> thetavec_av_y;
oaa36 5:9195756445e4 41 std::vector<int> thetavec_z;
oaa36 5:9195756445e4 42 std::vector<int> thetavec_av_z;
oaa36 5:9195756445e4 43 std::vector<int> sig;
oaa36 5:9195756445e4 44
oaa36 5:9195756445e4 45 x_data.assign (20,0);
oaa36 5:9195756445e4 46 y_data.assign (20,0);
oaa36 5:9195756445e4 47 z_data.assign (20,0);
oaa36 5:9195756445e4 48 //x_sig.assign (20,0);
oaa36 5:9195756445e4 49 //y_sig.assign (20,0);
oaa36 5:9195756445e4 50 //z_sig.assign (20,0);
oaa36 5:9195756445e4 51 thetavec_x.assign (20, 0);
oaa36 5:9195756445e4 52 thetavec_av_x.assign (20, 0);
oaa36 5:9195756445e4 53 thetavec_y.assign (20, 0);
oaa36 5:9195756445e4 54 thetavec_av_y.assign (20, 0);
oaa36 5:9195756445e4 55 thetavec_z.assign (20, 0);
oaa36 5:9195756445e4 56 thetavec_av_z.assign (20, 0);
oaa36 5:9195756445e4 57 sig.assign (20,0);
oaa36 5:9195756445e4 58
oaa36 5:9195756445e4 59 int threshold1 = 1.0105;
oaa36 5:9195756445e4 60 int threshold2 = 1.2639;
oaa36 5:9195756445e4 61 int threshold3 = 0.8398;
oaa36 5:9195756445e4 62 int threshold4 = 0.2050;
oaa36 5:9195756445e4 63 int threshold5 = 0.5905;
oaa36 5:9195756445e4 64
oaa36 5:9195756445e4 65 /*my_nrf24l01p.powerUp();
Owen 0:a51a6e7da590 66 my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
Owen 0:a51a6e7da590 67 my_nrf24l01p.setReceiveMode();
oaa36 5:9195756445e4 68 my_nrf24l01p.enable();*/
oaa36 5:9195756445e4 69
trisjph 3:5c7f4e5a7605 70 printf("MMA8451 ID: %d\n", acc.getWhoAmI());
oaa36 5:9195756445e4 71
Owen 0:a51a6e7da590 72 while (1) {
oaa36 5:9195756445e4 73 float x, y, z, sum, root;
oaa36 5:9195756445e4 74 float average_x, average_y, average_z;
oaa36 5:9195756445e4 75 float variance_x, variance_y, variance_z;
oaa36 5:9195756445e4 76 float std_deviation_x, std_deviation_y, std_deviation_z;
oaa36 5:9195756445e4 77 float total_x = 0, total_y = 0, total_z = 0;
oaa36 5:9195756445e4 78 float sum1_x = 0, sum1_y = 0, sum1_z = 0;
oaa36 5:9195756445e4 79 float average_x1, average_y1, average_z1;
oaa36 5:9195756445e4 80 float variance_x1, variance_y1, variance_z1;
oaa36 5:9195756445e4 81 float std_deviation_x1, std_deviation_y1, std_deviation_z1;
oaa36 5:9195756445e4 82 float total_x1 = 0, total_y1 = 0, total_z1 = 0;
oaa36 5:9195756445e4 83 float sum1_x1 = 0, sum1_y1 = 0, sum1_z1 = 0;
oaa36 5:9195756445e4 84 float sqr_sum_xyz, mag_sigXYZ;
oaa36 5:9195756445e4 85 float theta_average_z, theta_average_y, theta_average_x;
oaa36 5:9195756445e4 86 float theta_ratio_x, theta_ratio_y, theta_ratio_z;
oaa36 5:9195756445e4 87 float dtheta_z, dtheta_y, dtheta_x;
oaa36 5:9195756445e4 88 float sig_x, sig_y, sig_z;
oaa36 5:9195756445e4 89 float theta_z, theta_y, theta_x;
oaa36 5:9195756445e4 90 float sigratio_z, sigratio_y, sigratio_x;
oaa36 5:9195756445e4 91 float total_theta_z1, total_theta_y1, total_theta_x1;
oaa36 5:9195756445e4 92 float sig_ratio;
oaa36 5:9195756445e4 93 float total_sig, total_sig1;
oaa36 5:9195756445e4 94 float average_sig, average_sig1;
oaa36 5:9195756445e4 95 float variance_sig1, variance_sig;
oaa36 5:9195756445e4 96 float sum_sig1, sum_sig;
oaa36 5:9195756445e4 97 float sigA, sigA1;
oaa36 5:9195756445e4 98
trisjph 3:5c7f4e5a7605 99 x = abs(acc.getAccX());
oaa36 5:9195756445e4 100 //printf("%f", x);
trisjph 3:5c7f4e5a7605 101 y = abs(acc.getAccY());
trisjph 3:5c7f4e5a7605 102 z = abs(acc.getAccZ());
oaa36 5:9195756445e4 103 sum = pow(x, 2)+ pow(y, 2) + pow(z, 2);
oaa36 5:9195756445e4 104 root = sqrt(sum);
oaa36 5:9195756445e4 105 //printf("%f", root);
oaa36 5:9195756445e4 106
oaa36 5:9195756445e4 107 theta_z = acos(z/root);
oaa36 5:9195756445e4 108 //theta_y = acos(y/root);
oaa36 5:9195756445e4 109 //theta_x = acos(x/root);
oaa36 5:9195756445e4 110
oaa36 5:9195756445e4 111 x_data.push_back (x);
oaa36 5:9195756445e4 112 y_data.push_back (y);
oaa36 5:9195756445e4 113 z_data.push_back (z);
oaa36 5:9195756445e4 114 sig.push_back (root);
oaa36 5:9195756445e4 115
oaa36 5:9195756445e4 116 x_data.pop_back();
oaa36 5:9195756445e4 117 y_data.pop_back();
oaa36 5:9195756445e4 118 z_data.pop_back();
oaa36 5:9195756445e4 119 sig.pop_back ();
oaa36 5:9195756445e4 120
oaa36 5:9195756445e4 121 //printf ("x_data\n\r");
oaa36 5:9195756445e4 122 //for (int p = 0; p != x_data.size(); ++p)
oaa36 5:9195756445e4 123 //{
oaa36 5:9195756445e4 124 //cout << x_data[p] << "\n\r" << endl;
oaa36 5:9195756445e4 125 //cout << x_data.at(p) << "\n\r" << endl;
oaa36 5:9195756445e4 126 //}
oaa36 5:9195756445e4 127
oaa36 5:9195756445e4 128 thetavec_z.push_back (theta_z);
oaa36 5:9195756445e4 129 //thetavec_y.push_back (theta_y);
oaa36 5:9195756445e4 130 //thetavec_x.push_back (theta_x);
oaa36 5:9195756445e4 131
oaa36 5:9195756445e4 132 thetavec_z.pop_back ();
oaa36 5:9195756445e4 133 //thetavec_y.pop_back ();
oaa36 5:9195756445e4 134 //thetavec_x.pop_back ();
oaa36 5:9195756445e4 135
oaa36 5:9195756445e4 136 int i, n = 20, m = 10;
oaa36 5:9195756445e4 137
oaa36 5:9195756445e4 138 /* Compute mean */
oaa36 6:6710cd90a81c 139 for (i = 0; i < m; i++) {
oaa36 5:9195756445e4 140 total_x = total_x + x_data[i];
oaa36 5:9195756445e4 141 total_y = total_y + y_data[i];
oaa36 5:9195756445e4 142 total_z = total_z + z_data[i];
oaa36 5:9195756445e4 143 total_sig = total_sig + sig [i];
oaa36 5:9195756445e4 144 }
oaa36 5:9195756445e4 145
oaa36 5:9195756445e4 146 average_x = total_x / (float)m;
oaa36 5:9195756445e4 147 average_y = total_y / (float)m;
oaa36 5:9195756445e4 148 average_z = total_z / (float)m;
oaa36 5:9195756445e4 149 average_sig = total_sig / (float)m;
oaa36 5:9195756445e4 150
oaa36 6:6710cd90a81c 151 for (i = 10; i < n; i++) {
oaa36 5:9195756445e4 152 total_x1 = total_x1 + x_data[i];
oaa36 5:9195756445e4 153 total_y1 = total_y1 + y_data[i];
oaa36 5:9195756445e4 154 total_z1 = total_z1 + z_data[i];
oaa36 5:9195756445e4 155 total_sig1 = total_sig1 + sig[i];
oaa36 5:9195756445e4 156 }
oaa36 5:9195756445e4 157
oaa36 5:9195756445e4 158 average_x1 = total_x1 / (float)m;
oaa36 5:9195756445e4 159 average_y1 = total_y1 / (float)m;
oaa36 5:9195756445e4 160 average_z1 = total_z1 / (float)m;
oaa36 5:9195756445e4 161 average_sig1 = total_sig1 / (float)m;
oaa36 5:9195756445e4 162
oaa36 5:9195756445e4 163
oaa36 5:9195756445e4 164 /* Compute variance and standard deviation */
oaa36 6:6710cd90a81c 165 for (i = 0; i < m; i++) {
oaa36 5:9195756445e4 166 sum1_x = sum1_x + pow((x_data[i] - average_x), 2);
oaa36 5:9195756445e4 167 sum1_y = sum1_y + pow((y_data[i] - average_y), 2);
oaa36 5:9195756445e4 168 sum1_z = sum1_z + pow((z_data[i] - average_z), 2);
oaa36 5:9195756445e4 169 sum_sig = sum_sig + pow((sig[i] - average_sig), 2);
oaa36 5:9195756445e4 170 }
oaa36 5:9195756445e4 171 variance_x = sum1_x / (float)m;
oaa36 5:9195756445e4 172 variance_y = sum1_y / (float)m;
oaa36 5:9195756445e4 173 variance_z = sum1_z / (float)m;
oaa36 5:9195756445e4 174 variance_sig = sum_sig / (float)m;
oaa36 5:9195756445e4 175
oaa36 5:9195756445e4 176 std_deviation_x = sqrt(variance_x);
oaa36 5:9195756445e4 177 std_deviation_y = sqrt(variance_y);
oaa36 5:9195756445e4 178 std_deviation_z = sqrt(variance_z);
oaa36 5:9195756445e4 179 sigA = sqrt(variance_sig);
oaa36 5:9195756445e4 180
oaa36 6:6710cd90a81c 181 for (i = 10; i < n; i++) {
oaa36 5:9195756445e4 182 sum1_x1 = sum1_x1 + pow((x_data[i] - average_x1), 2);
oaa36 5:9195756445e4 183 sum1_y1 = sum1_y1 + pow((y_data[i] - average_y1), 2);
oaa36 5:9195756445e4 184 sum1_z1 = sum1_z1 + pow((z_data[i] - average_z1), 2);
oaa36 5:9195756445e4 185 sum_sig1 = sum_sig1 + pow((sig[i] - average_sig1), 2);
oaa36 5:9195756445e4 186 }
oaa36 5:9195756445e4 187 variance_x1 = sum1_x1 / (float)m;
oaa36 5:9195756445e4 188 variance_y1 = sum1_y1 / (float)m;
oaa36 5:9195756445e4 189 variance_z1 = sum1_z1 / (float)m;
oaa36 5:9195756445e4 190 variance_sig1 = sum_sig1 / (float)m;
oaa36 5:9195756445e4 191
oaa36 5:9195756445e4 192
oaa36 5:9195756445e4 193 std_deviation_x1 = sqrt(variance_x1);
oaa36 5:9195756445e4 194 std_deviation_y1 = sqrt(variance_y1);
oaa36 5:9195756445e4 195 std_deviation_z1 = sqrt(variance_z1);
oaa36 5:9195756445e4 196 sigA1 = sqrt(variance_sig);
oaa36 5:9195756445e4 197
oaa36 5:9195756445e4 198 sqr_sum_xyz = pow(std_deviation_x, 2) + pow(std_deviation_y, 2) + pow(std_deviation_z, 2);
oaa36 5:9195756445e4 199 mag_sigXYZ = sqrt(sqr_sum_xyz);
oaa36 5:9195756445e4 200
oaa36 5:9195756445e4 201 sig_ratio = sigA / sigA1;
oaa36 5:9195756445e4 202
oaa36 5:9195756445e4 203
oaa36 5:9195756445e4 204
oaa36 6:6710cd90a81c 205 for (i = 0; i < m; i++) {
oaa36 5:9195756445e4 206 total_theta_z1 = total_theta_z1 + thetavec_z[i];
oaa36 5:9195756445e4 207 //total_theta_y1 = total_theta_y1 + thetavec_y[i];
oaa36 5:9195756445e4 208 //total_theta_x1 = total_theta_x1 + thetavec_x[i];
oaa36 5:9195756445e4 209 }
oaa36 5:9195756445e4 210
oaa36 6:6710cd90a81c 211 theta_average_z = total_theta_z1 / (float)m;
oaa36 6:6710cd90a81c 212 //theta_average_y = total_theta_y1 / (float)m;
oaa36 6:6710cd90a81c 213 //theta_average_x = total_theta_x1 / (float)m;
oaa36 5:9195756445e4 214
oaa36 5:9195756445e4 215
oaa36 5:9195756445e4 216 thetavec_av_z.push_back(theta_average_z);
oaa36 5:9195756445e4 217 //thetavec_av_y.push_back(theta_average_y);
oaa36 5:9195756445e4 218 //thetavec_av_x.push_back(theta_average_x);
oaa36 5:9195756445e4 219
oaa36 5:9195756445e4 220
oaa36 5:9195756445e4 221 thetavec_av_z.pop_back();
oaa36 5:9195756445e4 222 //thetavec_av_y.pop_back();
oaa36 5:9195756445e4 223 //thetavec_av_x.pop_back();
oaa36 5:9195756445e4 224
oaa36 5:9195756445e4 225 theta_ratio_z = thetavec_av_z[0]/thetavec_av_z[10];
oaa36 5:9195756445e4 226 //theta_ratio_y = thetavec_av_y[0]/thetavec_av_y[10];
oaa36 5:9195756445e4 227 //theta_ratio_x = thetavec_av_x[0]/thetavec_av_x[10];
oaa36 5:9195756445e4 228
oaa36 5:9195756445e4 229 dtheta_z = thetavec_av_z[0]-thetavec_av_z[10];
oaa36 5:9195756445e4 230 // dtheta_y = thetavec_av_y[0]-thetavec_av_y[10];
oaa36 5:9195756445e4 231 // dtheta_x = thetavec_av_x[0]-thetavec_av_x[10];
oaa36 5:9195756445e4 232
oaa36 5:9195756445e4 233
oaa36 5:9195756445e4 234 float ON;
oaa36 5:9195756445e4 235
oaa36 6:6710cd90a81c 236 if ((sig_ratio > threshold1) || (theta_ratio_z > threshold2)) {
oaa36 6:6710cd90a81c 237 //have to add pressure sensor;
oaa36 6:6710cd90a81c 238 if (((theta_ratio_z > threshold3) || (dtheta_z > threshold4)) && ((sigA > threshold4) || (mag_sigXYZ > threshold5))) {
oaa36 6:6710cd90a81c 239 printf ("fall\n\r");
oaa36 5:9195756445e4 240 ON = 1;
oaa36 6:6710cd90a81c 241 } else {
oaa36 6:6710cd90a81c 242 printf ("no fall\n\r");
oaa36 5:9195756445e4 243 ON = 0;
oaa36 6:6710cd90a81c 244 }
oaa36 6:6710cd90a81c 245 } else {
oaa36 6:6710cd90a81c 246 printf ("no fall\n\r");
oaa36 5:9195756445e4 247 ON = 0;
oaa36 6:6710cd90a81c 248 }
oaa36 5:9195756445e4 249
oaa36 5:9195756445e4 250 //printf("X:%f,Y:%f,Z:%f\n", x, y, z);
oaa36 5:9195756445e4 251
oaa36 5:9195756445e4 252 //my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt );
oaa36 5:9195756445e4 253
oaa36 5:9195756445e4 254 //printf("sent");
oaa36 5:9195756445e4 255
oaa36 5:9195756445e4 256 wait(0.05);
Owen 0:a51a6e7da590 257 }
oaa36 6:6710cd90a81c 258 }