Diff: main.cpp

Revision:

10:63d223104806

Parent:

9:dcbd546412ea

--- a/main.cpp	Thu Dec 02 18:46:01 2021 +0000
+++ b/main.cpp	Tue Dec 14 03:56:53 2021 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "LSM9DS1.h"
-#include "stepcounter.h"
 #include "PulseSensor.h"
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
@@ -20,17 +19,19 @@
 */
 
 // IMU
-filter_avg_t acc_data;
-axis_info_t acc_sample;
-peak_value_t acc_peak;
-slid_reg_t acc_slid;
+//filter_avg_t acc_data;
+//axis_info_t acc_sample;
+//peak_value_t acc_peak;
+//slid_reg_t acc_slid;
+uint8_t step = 1;
+float threshold = 1.8;
 
 // skin temp
 AnalogIn skinTemp(p19);
 uint8_t temp_skin = 25;
 
 //Heart beat
-uint8_t bpm;
+uint8_t bpm = 70;
 
 //Human resistance
 AnalogIn gsr(p17);
@@ -68,85 +69,74 @@
 }
 
 
-void store_imu_data(float d, char* sp) {
-    int id = *(int*)&d;
-    for(int i = 0; i < 4; i++) {
-        *(sp+i) = id % 0xff;
-        id = id >> 8;
-    }
-}
-
-void update_IMU_data(void const *arg){
-    struct gyro gy;
-    struct acce ac;
-    while(1){
-        while(!IMU.accelAvailable());
-        IMU.readAccel();
-        while(!IMU.gyroAvailable());
-        IMU.readGyro();
-        
-        gy.x = IMU.calcGyro(IMU.gx);
-        gy.y = IMU.calcGyro(IMU.gy);
-        gy.z = IMU.calcGyro(IMU.gz);
-        ac.x = IMU.calcAccel(IMU.ax);
-        ac.y = IMU.calcAccel(IMU.ay);
-        ac.z = IMU.calcAccel(IMU.az);
-        
-        store_imu_data(gy.x, &imu_data[0]);
-        store_imu_data(gy.y, &imu_data[4]);
-        store_imu_data(gy.z, &imu_data[8]);
-        store_imu_data(ac.x, &imu_data[12]);
-        store_imu_data(ac.y, &imu_data[16]);
-        store_imu_data(ac.z, &imu_data[20]);
-        
-        Thread::wait(500);  
-    }
-}
-
 void step_counter(void const *arg) {
     
-    peak_value_init(&acc_peak);
+    //peak_value_init(&acc_peak);
     struct acce ac2;
-
-    while (1) {
-        uint16_t i = 0;
-        float temp = 0;
-
-        for (i = 0; i < FILTER_CNT; i++)
-        {
-            while(!IMU.accelAvailable());
-            //pc.printf("11\n");
-            IMU.readAccel();
-            ac2.x = IMU.calcAccel(IMU.ax);
-            ac2.y = IMU.calcAccel(IMU.ay);
-            ac2.z = IMU.calcAccel(IMU.az);
+    
+    float mag;
+    
+    timer.start();
+    
+    while(1) {
+        while(!IMU.accelAvailable());
+        //pc.printf("11\n");
+        IMU.readAccel();
+        ac2.x = IMU.calcAccel(IMU.ax);
+        ac2.y = IMU.calcAccel(IMU.ay);
+        ac2.z = IMU.calcAccel(IMU.az);
+        mag = sqrt(pow(ac2.x, 2) + pow(ac2.y, 2) + pow(ac2.z, 2));
+        
+        if (mag > threshold && timer.read_ms() > 300) {
+            //pc.printf("%d \n\r", timer.read_ms());
+            step += 1;
+            timer.stop();
+            timer.reset();
+            timer.start();
+        }
+        Thread::wait(5);
+    }
+    
             
-            temp = ac2.x * DATA_FACTOR;
-            acc_data.info[i].x = (short)(temp);
-
-            temp = ac2.y * DATA_FACTOR;
-            acc_data.info[i].y = (short)temp;
-
-            temp = ac2.z * DATA_FACTOR;
-            acc_data.info[i].z = (short)temp;
-            
-            Thread::wait(5);
-        }
-
-        filter_calculate(&acc_data, &acc_sample);
-
-        peak_update(&acc_peak, &acc_sample);
-
-        slid_update(&acc_slid, &acc_sample);
-
-        detect_step(&acc_peak, &acc_slid, &acc_sample);
-        
-        timer.stop();
-        if(timer.read_ms() <= 20)
-            Thread::wait(20 - timer.read_ms());
-        //Thread::wait(5);
-        
-    }
+    //while (1) {
+//        uint16_t i = 0;
+//        float temp = 0;
+//
+//        for (i = 0; i < FILTER_CNT; i++)
+//        {
+//            while(!IMU.accelAvailable());
+//            //pc.printf("11\n");
+//            IMU.readAccel();
+//            ac2.x = IMU.calcAccel(IMU.ax);
+//            ac2.y = IMU.calcAccel(IMU.ay);
+//            ac2.z = IMU.calcAccel(IMU.az);
+//            
+//            temp = ac2.x * DATA_FACTOR;
+//            acc_data.info[i].x = (short)(temp);
+//
+//            temp = ac2.y * DATA_FACTOR;
+//            acc_data.info[i].y = (short)temp;
+//
+//            temp = ac2.z * DATA_FACTOR;
+//            acc_data.info[i].z = (short)temp;
+//            
+//            Thread::wait(5);
+//        }
+//
+//        filter_calculate(&acc_data, &acc_sample);
+//
+//        peak_update(&acc_peak, &acc_sample);
+//
+//        slid_update(&acc_slid, &acc_sample);
+//
+//        detect_step(&acc_peak, &acc_slid, &acc_sample);
+//        
+//        timer.stop();
+//        if(timer.read_ms() <= 20)
+//            Thread::wait(20 - timer.read_ms());
+//        //Thread::wait(5);
+//        
+//    }
     
 }
 
@@ -183,8 +173,8 @@
         //pc.printf("Vt: %f\n\r", Vt);
         //pc.printf("R: %f\n\r", R);
         
-           temp_skin=(uint8_t)(T-273.15); 
-           //pc.printf("temp: %d\n", temp_skin);
+        temp_skin=(uint8_t)(T-273.15); 
+        //pc.printf("temp: %d\n", temp_skin);
         //pc.printf("Skin temp: %f C\n\r", T-273.15);
         Thread::wait(100);      
     }
@@ -230,6 +220,13 @@
     }
 }
 
+//void heartRate(void const *arg) {
+//    while (1) {
+//        bpm = rand() % 21 + 70;
+//        //bpm += (x - 5);
+//        Thread::wait(2000);
+//    }
+//}
 
 int main()
 {
@@ -246,24 +243,34 @@
     
     PulseSensor sensor(p15, sendDataToProcessing);
     sensor.start();
-
    
     Thread t1(step_counter);
     Thread t2(skin_temp);
     Thread t3(hum_R);
-    bpm = 60;
+    //Thread t4(heartRate);
     
     pc.printf("Main Loop\n");
     while(1) {
         
         dev.putc(0xff);
         dev.putc(temp_skin);
-        dev.putc(sensor.get_bpm());
+        dev.putc(bpm);
         dev.putc(Human_Resistance);
-        dev.putc(get_step());
+        dev.putc(step);
         
         dev.putc(0xff);
+        //pc.printf("%d\n\r", rand() % 31 + 60);
         Thread::wait(200);
         
     }
+//    struct acce ac2;
+//    while(1) {
+//        
+//        IMU.readAccel();
+//        ac2.x = IMU.calcAccel(IMU.ax);
+//        ac2.y = IMU.calcAccel(IMU.ay);
+//        ac2.z = IMU.calcAccel(IMU.az);
+//        pc.printf("%f \n\r", sqrt(pow(ac2.x, 2) + pow(ac2.y, 2) + pow(ac2.z, 2)));
+//        wait(0.1);
+//    }
 }