Diff: main.cpp

Revision:

11:cd28cb84fd81

Parent:

10:6a9de32601b1

--- a/main.cpp	Mon Nov 05 12:14:58 2018 +0000
+++ b/main.cpp	Fri Nov 16 02:09:00 2018 +0000
@@ -1,5 +1,3 @@
-#define DEBUG_STDIO 1
-
 #include "mbed.h"       //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
 #include "encoder.h"
 //#include "Mx28.h"
@@ -19,14 +17,8 @@
 
 Serial uart(USBTX, USBRX);
 //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
 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
 //LSM9DS1 imu2(PC_9,PA_8);   //SDA SCL
@@ -41,9 +33,8 @@
 
 // Timer
 Ticker timer1;
-float ITR_time1 = 100.0;  //timer interrupt unit：us  多少us計時一次 10毫秒
+float ITR_time1 = 4000.0;  //timer interrupt unit：us  多少us計時一次 4毫秒
 float Ts = ITR_time1/1000000;    //控馬達的某個時間參數 不用理他
-uint8_t flag;
 
 // EMG
 float lpf(float input, float output_old, float frequency);  //low pass filter
@@ -65,43 +56,31 @@
 
 // function
 void init_UART();
-void init_TIMER();
-void timer1_ITR();
+//void init_TIMER();
+//void timer1_ITR();
 void uart_tx(); // 傳data到電腦
 /**************************************************************************/
 int main()
 {
-//    LED = 1; // darken
 //    wait_ms(500);
     // initial sensor
-//    init_SPI_encoder();
-//    init_encoder();
+    init_SPI_encoder();
+    init_encoder();
     init_UART();
 
     init_IMU();
-    // initial uart
-
-
-
-//    wait_ms(500);
-
-//    led2 = 1;
-//    led2f = 0;
-//    LED = 0; // lighten
 
 //    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();
+      imu.readAccel();
+      imu.readGyro();
 //    imu2.readAccel();
 //    imu2.readGyro();
 
@@ -119,20 +98,18 @@
 //    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];
-//    }
+    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();
+//    uart.printf("x: %.4f, y: %.4f, z: %.4f\n\r", imu.ax, imu.ay, imu.az);
+    uart_tx();
 
-//    flag = 0;
-        wait_ms(500);
-//    test = 0;
+//        wait_ms(500);
     }
 }
 
@@ -156,23 +133,21 @@
     uart.baud(115200);
 }
 
-void init_TIMER()
-{
-    timer1.attach_us(&timer1_ITR, ITR_time1);
-}
+//void init_TIMER()
+//{
+//    timer1.attach_us(&timer1_ITR, ITR_time1);
+//}
 
-void timer1_ITR()    //開始讀取資料
-{
-//    test = 1;
-    led2 = !led2;
+//void timer1_ITR()    //開始讀取資料
+//{
     // EMG
     //emg_value = EMG.read();
 //    emg_value_f = lpf(emg_value,emg_value_f_old,15);
 //    emg_value_f_old = emg_value_f;
 
-    CS = 1;
     // IMU
-    imu.readAccel();
+//    imu.readAccel();
+    
 //    imu.readGyro();
 //    imu2.readAccel();
 //    imu2.readGyro();
@@ -200,10 +175,8 @@
 //    uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az);
     //uart_tx();
 
-//    flag = 0;
 //    wait_ms(1);
-//    test = 0;
-}
+//}
 
 void uart_tx()    //分割資料
 {
@@ -225,8 +198,8 @@
     s7.j = emg_value_f;
 //    s8.j = 1; // 若要傳的參數不足8個，則隨意打一個常數即可
 
-    T[2] = Acce_axis_data[0];  //0x5A;
-    T[3] = Acce_axis_data[0] >>8;
+    T[2] = s1.C[0];
+    T[3] = s1.C[1];
     T[4] = s2.C[0];
     T[5] = s2.C[1];
     T[6] = s3.C[0];
@@ -244,22 +217,22 @@
 
 
 
-//    uart.putc(T[0]);
-//    uart.putc(T[1]);
-//    uart.putc(T[2]);
-//    uart.putc(T[3]);
-//    uart.putc(T[4]);
-//    uart.putc(T[5]);
-//    uart.putc(T[6]);
-//    uart.putc(T[7]);
-//    uart.putc(T[8]);
-//    uart.putc(T[9]);
-//    uart.putc(T[10]);
-//    uart.putc(T[11]);
-//    uart.putc(T[12]);
-//    uart.putc(T[13]);
-//    uart.putc(T[14]);
-//    uart.putc(T[15]);
+    uart.putc(T[0]);
+    uart.putc(T[1]);
+    uart.putc(T[2]);
+    uart.putc(T[3]);
+    uart.putc(T[4]);
+    uart.putc(T[5]);
+    uart.putc(T[6]);
+    uart.putc(T[7]);
+    uart.putc(T[8]);
+    uart.putc(T[9]);
+    uart.putc(T[10]);
+    uart.putc(T[11]);
+    uart.putc(T[12]);
+    uart.putc(T[13]);
+    uart.putc(T[14]);
+    uart.putc(T[15]);
 
 //    while(1) {    //開始傳到USB
 //        if (uart.writeable() == 1) {