Jingtian Dang / Mbed 2 deprecated 4180project

ECE4180

Dependencies:   mbed PulseSensor mbed-rtos LSM9DS1_Library_cal

main.cpp@7:88d71c228407, 2021-12-01 (annotated)

Committer:
yutation
Date:
Wed Dec 01 00:20:32 2021 +0000
Revision:
7:88d71c228407
Parent:
6:0398d0fcc8cc
Child:
8:2d43385e7784
4180;

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 7:88d71c228407 5 #define PI 3.14159
yutation 7:88d71c228407 6 // Earth's magnetic field varies by location. Add or subtract
yutation 7:88d71c228407 7 // a declination to get a more accurate heading. Calculate
yutation 7:88d71c228407 8 // your's here:
yutation 7:88d71c228407 9 // http://www.ngdc.noaa.gov/geomag-web/#declination
yutation 7:88d71c228407 10 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
yutation 7:88d71c228407 11 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
yutation 7:88d71c228407 12 Timer timer;
yutation 7:88d71c228407 13
yutation 7:88d71c228407 14 //
yutation 7:88d71c228407 15 filter_avg_t acc_data;
yutation 7:88d71c228407 16 axis_info_t acc_sample;
yutation 7:88d71c228407 17 peak_value_t acc_peak;
yutation 7:88d71c228407 18 slid_reg_t acc_slid;
yutation 7:88d71c228407 19 uint8_t step_cnt = 0;
yutation 7:88d71c228407 20 //
yutation 7:88d71c228407 21 AnalogIn skinTemp(p19);
mbedAustin 2:a8dcb07a1d00 22
sam_grove 5:96cb82af9996 23 RawSerial pc(USBTX, USBRX);
yutation 7:88d71c228407 24 RawSerial dev(p9,p10);
sam_grove 5:96cb82af9996 25 DigitalOut led1(LED1);
sam_grove 5:96cb82af9996 26 DigitalOut led4(LED4);
mbedAustin 2:a8dcb07a1d00 27
yutation 7:88d71c228407 28 struct gyro {
yutation 7:88d71c228407 29 float x;
yutation 7:88d71c228407 30 float y;
yutation 7:88d71c228407 31 float z;
yutation 7:88d71c228407 32 };
yutation 7:88d71c228407 33
yutation 7:88d71c228407 34 struct acce {
yutation 7:88d71c228407 35 float x;
yutation 7:88d71c228407 36 float y;
yutation 7:88d71c228407 37 float z;
yutation 7:88d71c228407 38 };
yutation 7:88d71c228407 39
yutation 7:88d71c228407 40 char imu_data[24];
yutation 7:88d71c228407 41
yutation 7:88d71c228407 42
yutation 7:88d71c228407 43 char c[4];
yutation 7:88d71c228407 44 void aa(void const *arg){
yutation 7:88d71c228407 45 char d1 = '0';
yutation 7:88d71c228407 46 while(1){
yutation 7:88d71c228407 47 c[1] = d1;
yutation 7:88d71c228407 48 d1++;
yutation 7:88d71c228407 49 if(d1 >= '9')
yutation 7:88d71c228407 50 d1 = '0';
yutation 7:88d71c228407 51 Thread::wait(100);
yutation 7:88d71c228407 52 }
yutation 7:88d71c228407 53 }
yutation 7:88d71c228407 54
yutation 7:88d71c228407 55 void bb(void const *arg){
yutation 7:88d71c228407 56 char d2 = 'a';
yutation 7:88d71c228407 57 while(1){
yutation 7:88d71c228407 58 c[2] = d2;
yutation 7:88d71c228407 59 d2++;
yutation 7:88d71c228407 60 if(d2 >= 'z')
yutation 7:88d71c228407 61 d2 = 'a';
yutation 7:88d71c228407 62 Thread::wait(100);
yutation 7:88d71c228407 63 }
yutation 7:88d71c228407 64 }
yutation 7:88d71c228407 65
yutation 7:88d71c228407 66 void out(void const *arg){
yutation 7:88d71c228407 67 while(1){
yutation 7:88d71c228407 68 //int i = 1;
yutation 7:88d71c228407 69 //dev.putc(c[1]);
yutation 7:88d71c228407 70 //dev.putc(c[2]);
yutation 7:88d71c228407 71 //dev.putc(10);
yutation 7:88d71c228407 72 //pc.printf("step: %d\n", step_cnt);
yutation 7:88d71c228407 73 Thread::wait(100);
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 7:88d71c228407 116
yutation 7:88d71c228407 117 while (1)
yutation 7:88d71c228407 118 {
yutation 7:88d71c228407 119 timer.reset();
yutation 7:88d71c228407 120 timer.start();
yutation 7:88d71c228407 121 uint16_t i = 0;
yutation 7:88d71c228407 122 float temp = 0;
yutation 7:88d71c228407 123
yutation 7:88d71c228407 124 for (i = 0; i < FILTER_CNT; i++)
yutation 7:88d71c228407 125 {
yutation 7:88d71c228407 126 while(!IMU.accelAvailable());
yutation 7:88d71c228407 127 IMU.readAccel();
yutation 7:88d71c228407 128 ac2.x = IMU.calcAccel(IMU.ax);
yutation 7:88d71c228407 129 ac2.y = IMU.calcAccel(IMU.ay);
yutation 7:88d71c228407 130 ac2.z = IMU.calcAccel(IMU.az);
yutation 7:88d71c228407 131
yutation 7:88d71c228407 132 temp = ac2.x * DATA_FACTOR;
yutation 7:88d71c228407 133 acc_data.info[i].x = (short)(temp);
yutation 7:88d71c228407 134
yutation 7:88d71c228407 135 temp = ac2.y * DATA_FACTOR;
yutation 7:88d71c228407 136 acc_data.info[i].y = (short)temp;
yutation 7:88d71c228407 137
yutation 7:88d71c228407 138 temp = ac2.z * DATA_FACTOR;
yutation 7:88d71c228407 139 acc_data.info[i].z = (short)temp;
yutation 7:88d71c228407 140
yutation 7:88d71c228407 141 Thread::wait(5);
yutation 7:88d71c228407 142 }
yutation 7:88d71c228407 143
yutation 7:88d71c228407 144 filter_calculate(&acc_data, &acc_sample);
yutation 7:88d71c228407 145
yutation 7:88d71c228407 146 peak_update(&acc_peak, &acc_sample);
yutation 7:88d71c228407 147
yutation 7:88d71c228407 148 slid_update(&acc_slid, &acc_sample);
yutation 7:88d71c228407 149
yutation 7:88d71c228407 150 detect_step(&acc_peak, &acc_slid, &acc_sample);
yutation 7:88d71c228407 151
yutation 7:88d71c228407 152 timer.stop();
yutation 7:88d71c228407 153 if(timer.read_ms() <= 20)
yutation 7:88d71c228407 154 Thread::wait(20 - timer.read_ms());
mbedAustin 0:59bec1fd956e 155 }
yutation 7:88d71c228407 156
mbedAustin 0:59bec1fd956e 157 }
mbedAustin 4:ba9100d52e48 158
yutation 7:88d71c228407 159 void skin_temp(void const *arg) {
yutation 7:88d71c228407 160 float R1 = 12457; //thermistor resistance at 20C
yutation 7:88d71c228407 161 float R2 = 8052.1; //thermistor resistance at 30C
yutation 7:88d71c228407 162 float R3 = 5334.0; //thermistor resistance at 40C
yutation 7:88d71c228407 163
yutation 7:88d71c228407 164 float T1 = 293.15; //20C
yutation 7:88d71c228407 165 float T2 = 303.15; //30C
yutation 7:88d71c228407 166 float T3 = 313.15; //40C
yutation 7:88d71c228407 167
yutation 7:88d71c228407 168 float L1 = log(R1);
yutation 7:88d71c228407 169 float L2 = log(R2);
yutation 7:88d71c228407 170 float L3 = log(R3);
yutation 7:88d71c228407 171 float Y1 = 1/T1;
yutation 7:88d71c228407 172 float Y2 = 1/T2;
yutation 7:88d71c228407 173 float Y3 = 1/T3;
yutation 7:88d71c228407 174
yutation 7:88d71c228407 175 float g2 = (Y2-Y1)/(L2-L1);
yutation 7:88d71c228407 176 float g3 = (Y3-Y1)/(L3-L1);
yutation 7:88d71c228407 177
yutation 7:88d71c228407 178 float C = (g3-g2)/(L3-L2)*(1/(L1+L2+L3));
yutation 7:88d71c228407 179 float B = g2 - C*(L1*L1 + L1*L2 + L2*L2);
yutation 7:88d71c228407 180 float A = Y1 - L1*(B + L1*L1*C);
yutation 7:88d71c228407 181
yutation 7:88d71c228407 182 float Vt;
yutation 7:88d71c228407 183 while(1) {
yutation 7:88d71c228407 184 Vt = skinTemp;
yutation 7:88d71c228407 185 //myled = skinTemp;
yutation 7:88d71c228407 186 float R = 9900*(1/Vt - 1); //9900 is the resistance of R1 in voltage divider
yutation 7:88d71c228407 187
yutation 7:88d71c228407 188 float T = 1/(A + B*log(R) + C*log(R)*log(R)*log(R));
yutation 7:88d71c228407 189 //pc.printf("Vt: %f\n\r", 1 - Vt);
yutation 7:88d71c228407 190 //pc.printf("R: %f\n\r", R);
yutation 7:88d71c228407 191
yutation 7:88d71c228407 192 //pc.printf("Skin temp: %f\n\r", T-273.15);
yutation 7:88d71c228407 193 Thread::wait(1000);
yutation 7:88d71c228407 194 }
yutation 7:88d71c228407 195
yutation 7:88d71c228407 196 }
yutation 7:88d71c228407 197
yutation 7:88d71c228407 198
mbedAustin 4:ba9100d52e48 199 int main()
mbedAustin 4:ba9100d52e48 200 {
yutation 7:88d71c228407 201
mbedAustin 4:ba9100d52e48 202 pc.baud(9600);
mbedAustin 4:ba9100d52e48 203 dev.baud(9600);
yutation 7:88d71c228407 204
yutation 7:88d71c228407 205 IMU.begin();
yutation 7:88d71c228407 206 if (!IMU.begin()) {
yutation 7:88d71c228407 207 pc.printf("Failed to communicate with LSM9DS1.\n");
yutation 7:88d71c228407 208 }
yutation 7:88d71c228407 209 IMU.calibrate(1);
mbedAustin 4:ba9100d52e48 210
yutation 7:88d71c228407 211
yutation 7:88d71c228407 212 Thread t1(step_counter);
yutation 7:88d71c228407 213 Thread t2(skin_temp);
yutation 7:88d71c228407 214 //Thread t2(bb);
yutation 7:88d71c228407 215 //Thread t3(out);
sam_grove 5:96cb82af9996 216
yutation 7:88d71c228407 217
mbedAustin 4:ba9100d52e48 218 while(1) {
yutation 7:88d71c228407 219
yutation 7:88d71c228407 220 dev.putc(0xff);
yutation 7:88d71c228407 221 dev.putc('a');
yutation 7:88d71c228407 222 dev.putc('b');
yutation 7:88d71c228407 223 dev.putc('\n')
yutation 7:88d71c228407 224 dev.putc(0xff);
yutation 7:88d71c228407 225 Thread::wait(10000);
yutation 7:88d71c228407 226
mbedAustin 4:ba9100d52e48 227 }
mbedAustin 4:ba9100d52e48 228 }