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@8:81a97ad26339, 2015-07-24 (annotated)

Committer:
oaa36
Date:
Fri Jul 24 14:07:46 2015 +0000
Revision:
8:81a97ad26339
Parent:
5:9195756445e4 
For Josie

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