Jingtian Dang / Mbed 2 deprecated 4180project

ECE4180

Dependencies:   mbed PulseSensor mbed-rtos LSM9DS1_Library_cal

main.cpp@8:2d43385e7784, 2021-12-02 (annotated)

Committer:
yutation
Date:
Thu Dec 02 00:58:37 2021 +0000
Revision:
8:2d43385e7784
Parent:
7:88d71c228407 
Project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedAustin 0:59bec1fd956e 1 #include "mbed.h"
yutation 7:88d71c228407 2 #include "rtos.h"
yutation 7:88d71c228407 3 #include "LSM9DS1.h"
yutation 7:88d71c228407 4 #include "stepcounter.h"
yutation 8:2d43385e7784 5 #include "PulseSensor.h"
yutation 7:88d71c228407 6 #define PI 3.14159
yutation 7:88d71c228407 7 // Earth's magnetic field varies by location. Add or subtract
yutation 7:88d71c228407 8 // a declination to get a more accurate heading. Calculate
yutation 7:88d71c228407 9 // your's here:
yutation 7:88d71c228407 10 // http://www.ngdc.noaa.gov/geomag-web/#declination
yutation 7:88d71c228407 11 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
yutation 7:88d71c228407 12 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
yutation 7:88d71c228407 13 Timer timer;
yutation 7:88d71c228407 14
yutation 8:2d43385e7784 15 /*
yutation 8:2d43385e7784 16 temp
yutation 8:2d43385e7784 17 pulse
yutation 8:2d43385e7784 18 ppm
yutation 8:2d43385e7784 19 step
yutation 8:2d43385e7784 20 */
yutation 8:2d43385e7784 21
yutation 8:2d43385e7784 22 // IMU
yutation 7:88d71c228407 23 filter_avg_t acc_data;
yutation 7:88d71c228407 24 axis_info_t acc_sample;
yutation 7:88d71c228407 25 peak_value_t acc_peak;
yutation 7:88d71c228407 26 slid_reg_t acc_slid;
yutation 8:2d43385e7784 27 //uint8_t step_cnt = 1;
yutation 8:2d43385e7784 28
yutation 8:2d43385e7784 29 // skin temp
yutation 7:88d71c228407 30 AnalogIn skinTemp(p19);
yutation 8:2d43385e7784 31 uint8_t temp_skin = 48;
mbedAustin 2:a8dcb07a1d00 32
yutation 8:2d43385e7784 33 //Pulse
yutation 8:2d43385e7784 34 char pulse_symbol;
yutation 8:2d43385e7784 35 char pulse_data;
yutation 8:2d43385e7784 36
yutation 8:2d43385e7784 37 //H_R
yutation 8:2d43385e7784 38 AnalogIn gsr(p17);
yutation 8:2d43385e7784 39 AnalogIn sig(p18);
yutation 8:2d43385e7784 40
yutation 8:2d43385e7784 41 uint8_t hr = 1;
yutation 8:2d43385e7784 42 float gsrValue = 0;
yutation 8:2d43385e7784 43 float phasic = 0;
yutation 8:2d43385e7784 44 float baseline = 0;
yutation 8:2d43385e7784 45 int on = 1, off = 0;
yutation 8:2d43385e7784 46
yutation 8:2d43385e7784 47 Serial pc(USBTX, USBRX);
yutation 7:88d71c228407 48 RawSerial dev(p9,p10);
sam_grove 5:96cb82af9996 49 DigitalOut led1(LED1);
sam_grove 5:96cb82af9996 50 DigitalOut led4(LED4);
mbedAustin 2:a8dcb07a1d00 51
yutation 7:88d71c228407 52 struct gyro {
yutation 7:88d71c228407 53 float x;
yutation 7:88d71c228407 54 float y;
yutation 7:88d71c228407 55 float z;
yutation 7:88d71c228407 56 };
yutation 7:88d71c228407 57
yutation 7:88d71c228407 58 struct acce {
yutation 7:88d71c228407 59 float x;
yutation 7:88d71c228407 60 float y;
yutation 7:88d71c228407 61 float z;
yutation 7:88d71c228407 62 };
yutation 7:88d71c228407 63
yutation 7:88d71c228407 64 char imu_data[24];
yutation 7:88d71c228407 65
yutation 7:88d71c228407 66
yutation 8:2d43385e7784 67
yutation 7:88d71c228407 68
yutation 8:2d43385e7784 69 void sendDataToProcessing(char symbol, int data)
yutation 8:2d43385e7784 70 {
yutation 8:2d43385e7784 71 pc.printf("%c%d\r\n", symbol, data);
yutation 8:2d43385e7784 72 pulse_symbol = symbol;
yutation 8:2d43385e7784 73 pulse_data = data;
yutation 7:88d71c228407 74 }
yutation 7:88d71c228407 75
yutation 7:88d71c228407 76
yutation 7:88d71c228407 77 void store_imu_data(float d, char* sp) {
yutation 7:88d71c228407 78 int id = *(int*)&d;
yutation 7:88d71c228407 79 for(int i = 0; i < 4; i++) {
yutation 7:88d71c228407 80 *(sp+i) = id % 0xff;
yutation 7:88d71c228407 81 id = id >> 8;
yutation 7:88d71c228407 82 }
yutation 7:88d71c228407 83 }
yutation 7:88d71c228407 84
yutation 7:88d71c228407 85 void update_IMU_data(void const *arg){
yutation 7:88d71c228407 86 struct gyro gy;
yutation 7:88d71c228407 87 struct acce ac;
yutation 7:88d71c228407 88 while(1){
yutation 7:88d71c228407 89 while(!IMU.accelAvailable());
yutation 7:88d71c228407 90 IMU.readAccel();
yutation 7:88d71c228407 91 while(!IMU.gyroAvailable());
yutation 7:88d71c228407 92 IMU.readGyro();
yutation 7:88d71c228407 93
yutation 7:88d71c228407 94 gy.x = IMU.calcGyro(IMU.gx);
yutation 7:88d71c228407 95 gy.y = IMU.calcGyro(IMU.gy);
yutation 7:88d71c228407 96 gy.z = IMU.calcGyro(IMU.gz);
yutation 7:88d71c228407 97 ac.x = IMU.calcAccel(IMU.ax);
yutation 7:88d71c228407 98 ac.y = IMU.calcAccel(IMU.ay);
yutation 7:88d71c228407 99 ac.z = IMU.calcAccel(IMU.az);
yutation 7:88d71c228407 100
yutation 7:88d71c228407 101 store_imu_data(gy.x, &imu_data[0]);
yutation 7:88d71c228407 102 store_imu_data(gy.y, &imu_data[4]);
yutation 7:88d71c228407 103 store_imu_data(gy.z, &imu_data[8]);
yutation 7:88d71c228407 104 store_imu_data(ac.x, &imu_data[12]);
yutation 7:88d71c228407 105 store_imu_data(ac.y, &imu_data[16]);
yutation 7:88d71c228407 106 store_imu_data(ac.z, &imu_data[20]);
yutation 7:88d71c228407 107
yutation 7:88d71c228407 108 Thread::wait(500);
sam_grove 5:96cb82af9996 109 }
sam_grove 5:96cb82af9996 110 }
sam_grove 5:96cb82af9996 111
yutation 7:88d71c228407 112 void step_counter(void const *arg) {
yutation 7:88d71c228407 113
yutation 7:88d71c228407 114 peak_value_init(&acc_peak);
yutation 7:88d71c228407 115 struct acce ac2;
yutation 8:2d43385e7784 116 //pc.printf("enter loop!!\n");
yutation 7:88d71c228407 117
yutation 7:88d71c228407 118 while (1)
yutation 7:88d71c228407 119 {
yutation 8:2d43385e7784 120 //pc.printf("00\n");
yutation 8:2d43385e7784 121 //timer.reset();
yutation 8:2d43385e7784 122 //timer.start();
yutation 7:88d71c228407 123 uint16_t i = 0;
yutation 7:88d71c228407 124 float temp = 0;
yutation 7:88d71c228407 125
yutation 7:88d71c228407 126 for (i = 0; i < FILTER_CNT; i++)
yutation 7:88d71c228407 127 {
yutation 7:88d71c228407 128 while(!IMU.accelAvailable());
yutation 8:2d43385e7784 129 //pc.printf("11\n");
yutation 7:88d71c228407 130 IMU.readAccel();
yutation 7:88d71c228407 131 ac2.x = IMU.calcAccel(IMU.ax);
yutation 7:88d71c228407 132 ac2.y = IMU.calcAccel(IMU.ay);
yutation 7:88d71c228407 133 ac2.z = IMU.calcAccel(IMU.az);
yutation 7:88d71c228407 134
yutation 7:88d71c228407 135 temp = ac2.x * DATA_FACTOR;
yutation 7:88d71c228407 136 acc_data.info[i].x = (short)(temp);
yutation 7:88d71c228407 137
yutation 7:88d71c228407 138 temp = ac2.y * DATA_FACTOR;
yutation 7:88d71c228407 139 acc_data.info[i].y = (short)temp;
yutation 7:88d71c228407 140
yutation 7:88d71c228407 141 temp = ac2.z * DATA_FACTOR;
yutation 7:88d71c228407 142 acc_data.info[i].z = (short)temp;
yutation 7:88d71c228407 143
yutation 7:88d71c228407 144 Thread::wait(5);
yutation 7:88d71c228407 145 }
yutation 7:88d71c228407 146
yutation 7:88d71c228407 147 filter_calculate(&acc_data, &acc_sample);
yutation 7:88d71c228407 148
yutation 7:88d71c228407 149 peak_update(&acc_peak, &acc_sample);
yutation 7:88d71c228407 150
yutation 7:88d71c228407 151 slid_update(&acc_slid, &acc_sample);
yutation 7:88d71c228407 152
yutation 7:88d71c228407 153 detect_step(&acc_peak, &acc_slid, &acc_sample);
yutation 7:88d71c228407 154
yutation 7:88d71c228407 155 timer.stop();
yutation 7:88d71c228407 156 if(timer.read_ms() <= 20)
yutation 7:88d71c228407 157 Thread::wait(20 - timer.read_ms());
yutation 8:2d43385e7784 158 //Thread::wait(5);
yutation 8:2d43385e7784 159
mbedAustin 0:59bec1fd956e 160 }
yutation 7:88d71c228407 161
mbedAustin 0:59bec1fd956e 162 }
mbedAustin 4:ba9100d52e48 163
yutation 7:88d71c228407 164 void skin_temp(void const *arg) {
yutation 8:2d43385e7784 165 float R1 = 4700*1.4773; //thermistor resistance at 15C
yutation 8:2d43385e7784 166 float R2 = 4700; //thermistor resistance at 25C
yutation 8:2d43385e7784 167 float R3 = 4700*0.69105; //thermistor resistance at 35C
yutation 8:2d43385e7784 168
yutation 8:2d43385e7784 169 float T1 = 288.15; //15C
yutation 8:2d43385e7784 170 float T2 = 298.15; //25C
yutation 8:2d43385e7784 171 float T3 = 308.15; //35C
yutation 7:88d71c228407 172
yutation 7:88d71c228407 173 float L1 = log(R1);
yutation 7:88d71c228407 174 float L2 = log(R2);
yutation 7:88d71c228407 175 float L3 = log(R3);
yutation 7:88d71c228407 176 float Y1 = 1/T1;
yutation 7:88d71c228407 177 float Y2 = 1/T2;
yutation 7:88d71c228407 178 float Y3 = 1/T3;
yutation 7:88d71c228407 179
yutation 7:88d71c228407 180 float g2 = (Y2-Y1)/(L2-L1);
yutation 7:88d71c228407 181 float g3 = (Y3-Y1)/(L3-L1);
yutation 7:88d71c228407 182
yutation 7:88d71c228407 183 float C = (g3-g2)/(L3-L2)*(1/(L1+L2+L3));
yutation 7:88d71c228407 184 float B = g2 - C*(L1*L1 + L1*L2 + L2*L2);
yutation 7:88d71c228407 185 float A = Y1 - L1*(B + L1*L1*C);
yutation 7:88d71c228407 186
yutation 7:88d71c228407 187 float Vt;
yutation 7:88d71c228407 188 while(1) {
yutation 7:88d71c228407 189 Vt = skinTemp;
yutation 8:2d43385e7784 190 //float R = 9900*(1/Vt - 1); //9900 is the resistance of R1 in voltage divider
yutation 8:2d43385e7784 191 float R = 4900 * Vt / (1 - Vt);
yutation 7:88d71c228407 192
yutation 7:88d71c228407 193 float T = 1/(A + B*log(R) + C*log(R)*log(R)*log(R));
yutation 8:2d43385e7784 194 //pc.printf("Vt: %f\n\r", Vt);
yutation 7:88d71c228407 195 //pc.printf("R: %f\n\r", R);
yutation 8:2d43385e7784 196
yutation 8:2d43385e7784 197 temp_skin=(uint8_t)(T-273.15);
yutation 8:2d43385e7784 198 //pc.printf("temp: %d\n", temp_skin);
yutation 8:2d43385e7784 199 //pc.printf("Skin temp: %f C\n\r", T-273.15);
yutation 8:2d43385e7784 200 Thread::wait(100);
yutation 7:88d71c228407 201 }
yutation 7:88d71c228407 202
yutation 7:88d71c228407 203 }
yutation 7:88d71c228407 204
yutation 7:88d71c228407 205
yutation 8:2d43385e7784 206
yutation 8:2d43385e7784 207
yutation 8:2d43385e7784 208
yutation 8:2d43385e7784 209 // calculate baseline to compare against
yutation 8:2d43385e7784 210 void Get_Baseline(void)
yutation 8:2d43385e7784 211 {
yutation 8:2d43385e7784 212 double sum = 0;
yutation 8:2d43385e7784 213 wait(1);
yutation 8:2d43385e7784 214 for(int i=0; i<500; i++) {
yutation 8:2d43385e7784 215 gsrValue = sig;
yutation 8:2d43385e7784 216 sum += gsrValue ;
yutation 8:2d43385e7784 217 wait(0.005);
yutation 8:2d43385e7784 218 }
yutation 8:2d43385e7784 219 baseline = sum/500;
yutation 8:2d43385e7784 220 //printf("baseline = %f\n\r", baseline);
yutation 8:2d43385e7784 221 }
yutation 8:2d43385e7784 222
yutation 8:2d43385e7784 223 // main loop, compare against baseline
yutation 8:2d43385e7784 224 // sound buzzer if a >5% change happens
yutation 8:2d43385e7784 225 void hum_R(void const *arg)
yutation 8:2d43385e7784 226 {
yutation 8:2d43385e7784 227 float delta;
yutation 8:2d43385e7784 228 float Serial_Port_Reading;
yutation 8:2d43385e7784 229 int Human_Resistance;
yutation 8:2d43385e7784 230 Get_Baseline();
yutation 8:2d43385e7784 231
yutation 8:2d43385e7784 232 while(1) {
yutation 8:2d43385e7784 233 gsrValue = gsr;
yutation 8:2d43385e7784 234 phasic = sig;
yutation 8:2d43385e7784 235 delta = gsrValue - phasic;
yutation 8:2d43385e7784 236 if(abs(delta) > 0.05) {
yutation 8:2d43385e7784 237 gsrValue = gsr;
yutation 8:2d43385e7784 238 delta = baseline - gsrValue;
yutation 8:2d43385e7784 239 }
yutation 8:2d43385e7784 240 Human_Resistance = 254* (phasic);
yutation 8:2d43385e7784 241 //pc.printf("HR!!: %f\n", phasic);
yutation 8:2d43385e7784 242 //Human_Resistance = ((1024+2*Serial_Port_Reading)*10000)/(512-Serial_Port_Reading);
yutation 8:2d43385e7784 243 //printf("%f\n\r", delta); // print for phasic
yutation 8:2d43385e7784 244 //printf("%d\n\r", Human_Resistance); // print for gsrValue
yutation 8:2d43385e7784 245 hr = Human_Resistance;
yutation 8:2d43385e7784 246 Thread::wait(100);
yutation 8:2d43385e7784 247 }
yutation 8:2d43385e7784 248 }
yutation 8:2d43385e7784 249
yutation 8:2d43385e7784 250
yutation 8:2d43385e7784 251
mbedAustin 4:ba9100d52e48 252 int main()
mbedAustin 4:ba9100d52e48 253 {
yutation 7:88d71c228407 254
yutation 8:2d43385e7784 255 //pc.baud(9600);
yutation 8:2d43385e7784 256 //dev.baud(9600);
yutation 7:88d71c228407 257
yutation 7:88d71c228407 258 IMU.begin();
yutation 7:88d71c228407 259 if (!IMU.begin()) {
yutation 7:88d71c228407 260 pc.printf("Failed to communicate with LSM9DS1.\n");
yutation 7:88d71c228407 261 }
yutation 7:88d71c228407 262 IMU.calibrate(1);
yutation 8:2d43385e7784 263 pc.printf("IMU end\n");
yutation 8:2d43385e7784 264
yutation 8:2d43385e7784 265 PulseSensor sensor(p15, sendDataToProcessing);
yutation 8:2d43385e7784 266 //sensor.start();
mbedAustin 4:ba9100d52e48 267
yutation 7:88d71c228407 268
yutation 7:88d71c228407 269 Thread t1(step_counter);
yutation 7:88d71c228407 270 Thread t2(skin_temp);
yutation 8:2d43385e7784 271 Thread t3(hum_R);
yutation 8:2d43385e7784 272 char aaa = 1;
yutation 8:2d43385e7784 273 char bbb = 2;
yutation 8:2d43385e7784 274 char ccc = 11;
yutation 8:2d43385e7784 275 char ddd = 4;
yutation 7:88d71c228407 276
yutation 8:2d43385e7784 277 pc.printf("Main Loop\n");
mbedAustin 4:ba9100d52e48 278 while(1) {
yutation 8:2d43385e7784 279
yutation 7:88d71c228407 280
yutation 7:88d71c228407 281 dev.putc(0xff);
yutation 8:2d43385e7784 282 //pc.putc(temp_skin);
yutation 8:2d43385e7784 283 dev.putc(temp_skin);
yutation 8:2d43385e7784 284 dev.putc(ccc);
yutation 8:2d43385e7784 285 //dev.putc(pulse_symbol);
yutation 8:2d43385e7784 286
yutation 8:2d43385e7784 287 dev.putc(hr);
yutation 8:2d43385e7784 288 dev.putc(get_step());
yutation 8:2d43385e7784 289
yutation 7:88d71c228407 290 dev.putc(0xff);
yutation 8:2d43385e7784 291 //pc.putc('b');
yutation 8:2d43385e7784 292 //pc.printf("step_cnt: %d\n",get_step());
yutation 8:2d43385e7784 293 Thread::wait(200);
yutation 7:88d71c228407 294
mbedAustin 4:ba9100d52e48 295 }
mbedAustin 4:ba9100d52e48 296 }