Diff: main.cpp

Revision:

10:6a9de32601b1

Parent:

9:07de3af99031

Child:

11:cd28cb84fd81

--- a/main.cpp	Mon Nov 05 09:29:20 2018 +0000
+++ b/main.cpp	Mon Nov 05 12:14:58 2018 +0000
@@ -21,10 +21,11 @@
 //Serial uart(D10,D2);            // TX : D10     RX : D2           // bluetooth
 DigitalOut LED(A4);            // check if the code is running
 DigitalOut led2(PA_5);            // check if the code is running interrupt
-uint8_t led2f;                 //測試用
-AnalogIn EMG(PC_4);            //肌電訊號
+AnalogIn EMG(PC_4);            //
 DigitalOut test(PC_8);
 DigitalOut CS(PA_8);
+uint8_t led2f;                 //測試用
+
 //AnalogOut test2(PA_5);
 /*IMU*******************************************************************/
 LSM9DS1 imu(D14, D15);    //SDA SCL
@@ -75,9 +76,12 @@
     // initial sensor
 //    init_SPI_encoder();
 //    init_encoder();
+    init_UART();
+
     init_IMU();
     // initial uart
-    init_UART();
+
+
 
 //    wait_ms(500);
 
@@ -85,15 +89,66 @@
 //    led2f = 0;
 //    LED = 0; // lighten
 
-    init_TIMER();
+//    init_TIMER();
+
+    while(1) {
+        //    test = 1;
+        led2 = !led2;
+        // EMG
+        //emg_value = EMG.read();
+//    emg_value_f = lpf(emg_value,emg_value_f_old,15);
+//    emg_value_f_old = emg_value_f;
+
+        // IMU
+        imu.readAccel();
+//    imu.readGyro();
+//    imu2.readAccel();
+//    imu2.readGyro();
+
+        Acce_axis_data[0] = imu.ax*Acce_gain_x;
+        Acce_axis_data[1] = imu.ay*Acce_gain_y;
+        Acce_axis_data[2] = imu.az*Acce_gain_z;
+//    Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
+//    Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
+//    Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
 
-    while(1) {}
+        Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
+        Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
+        Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
+//    Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
+//    Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
+//    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);
+//        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: %.2f, y: %.2f, z: %.2f\n\r",imu.ax, imu.ay, imu.az);
+//    uart_tx();
+
+//    flag = 0;
+        wait_ms(500);
+//    test = 0;
+    }
 }
 
 void init_IMU()
 {
-    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
+    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
 //    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()
@@ -142,7 +197,7 @@
 //        Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
 //        Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
 //    }
-    uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az);
+//    uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az);
     //uart_tx();
 
 //    flag = 0;
@@ -184,10 +239,10 @@
     T[13] = s6.C[1];
     T[14] = s7.C[0];
     T[15] = s7.C[1];
-    
-    
-    
-    
+
+
+
+
 
 //    uart.putc(T[0]);
 //    uart.putc(T[1]);