Diff: main.cpp

Revision:

12:a197e7eed8ab

Parent:

11:cd28cb84fd81

--- a/main.cpp	Fri Nov 16 02:09:00 2018 +0000
+++ b/main.cpp	Wed Dec 19 12:35:03 2018 +0000
@@ -17,7 +17,7 @@
 
 Serial uart(USBTX, USBRX);
 //Serial uart(D10,D2);            // TX : D10     RX : D2           // bluetooth
-AnalogIn EMG(PC_4);            //
+AnalogIn EMG(PC_3);            //
 
 /*IMU*******************************************************************/
 LSM9DS1 imu(D14, D15);    //SDA SCL
@@ -26,8 +26,10 @@
 int16_t Gyro_axis_data[6] = {0};     // X, Y, Z axis
 int16_t Acce_axis_data[6] = {0};     // X, Y, Z axis
 float Acce_axis_data_f[6] = {0};     //_f代表經過低通濾波的資料
+float Acce_axis_data_old[6] = {0}; 
 float Acce_axis_data_f_old[6] = {0};
 float Gyro_axis_data_f[6] = {0};
+float Gyro_axis_data_old[6] = {0};
 float Gyro_axis_data_f_old[6] = {0};
 
 
@@ -37,10 +39,11 @@
 float Ts = ITR_time1/1000000;    //控馬達的某個時間參數 不用理他
 
 // EMG
-float lpf(float input, float output_old, float frequency);  //low pass filter
+float lpf(float input, float input_old, float output_old, float frequency);  //low pass filter
 float emg_value;
 float emg_value_f;
 float emg_value_f_old;
+float emg_value_old;
 float Tf = ITR_time1/1000000;     // 低通濾波的採樣週期
 
 // uart_tx
@@ -74,9 +77,11 @@
 
     while(1) {
         // EMG
-        //emg_value = EMG.read();
-//    emg_value_f = lpf(emg_value,emg_value_f_old,15);
-//    emg_value_f_old = emg_value_f;
+        emg_value = EMG.read();
+    emg_value_f = lpf(emg_value,emg_value_old,emg_value_f_old,15);
+    emg_value_f_old = emg_value_f;
+    emg_value_old = emg_value;
+
 
         // IMU
       imu.readAccel();
@@ -99,13 +104,15 @@
 //    Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
 
     for(i=0; i<6; i++) {
-        Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
-        Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
-        Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
+        Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_old[i],Acce_axis_data_f_old[i],15);
+        Acce_axis_data_old[i] = Acce_axis_data[i]; // input_old
+        Acce_axis_data_f_old[i] = Acce_axis_data_f[i]; // output_old
+        Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i], Gyro_axis_data_old[i], Gyro_axis_data_f_old[i],15);
+        Gyro_axis_data_old[i] = Gyro_axis_data[i];
         Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
     }
-//        int16_t shit2 = imu.ax_raw;
-//        uart.printf("%d\n\r", shit2);
+
+
 //    uart.printf("x: %.4f, y: %.4f, z: %.4f\n\r", imu.ax, imu.ay, imu.az);
     uart_tx();
 
@@ -115,17 +122,11 @@
 
 void init_IMU()
 {
-    uint16_t shit;
-    shit = imu.begin(imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625); //imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625
+
+    imu.begin(imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625); //imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625
 //    imu2.begin(imu.G_SCALE_500DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_476_BW_21,imu.A_ODR_476,imu.M_ODR_0625);
-    if(shit != 0x683D) {
-        while(1) {
-            uart.printf("IMU error\n\r");
-        }
-    }
+   
     uart.printf("IMU init success\n\r");
-
-//    uart.printf("%d\n\r", shit);
 }
 
 void init_UART()
@@ -246,11 +247,11 @@
 //    }
 }
 
-float lpf(float input, float output_old, float frequency)
+float lpf(float input, float input_old, float output_old, float frequency)
 {
     float output = 0;
-
-    output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
+    float a = frequency*Tf;
+    output = ((a+2)*output_old + a*input + a*input_old)/(3*a+2);
 
     return output;
 }