File content as of revision 10:6a9de32601b1:

#define DEBUG_STDIO 1

#include "mbed.h"       //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
#include "encoder.h"
//#include "Mx28.h"
//#include "PID.h"
#include "LSM9DS1.h"

//********************* Dynamxiel ******************************
#define SERVO_ID 0x01               // ID of which we will set Dynamixel too 
#define SERVO_ControlPin A2       // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
#define SERVO_SET_Baudrate 1000000  // Baud rate speed which the Dynamixel will be set too (1Mbps)
#define TxPin A0
#define RxPin A1
#define CW_LIMIT_ANGLE 1        // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
#define CCW_LIMIT_ANGLE 4095       // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
#define _PI 3.14159265f
//***************************************************************

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
void init_IMU();
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_f_old[6] = {0};
float Gyro_axis_data_f[6] = {0};
float Gyro_axis_data_f_old[6] = {0};


// Timer
Ticker timer1;
float ITR_time1 = 100.0;  //timer interrupt unit：us  多少us計時一次 10毫秒
float Ts = ITR_time1/1000000;    //控馬達的某個時間參數 不用理他
uint8_t flag;

// EMG
float lpf(float input, float output_old, float frequency);  //low pass filter
float emg_value;
float emg_value_f;
float emg_value_f_old;
float Tf = ITR_time1/1000000;     // 低通濾波的採樣週期

// uart_tx
union splitter {   //將data切割為兩個byte
    short j;
    char C[2];
    // C[0] is lowbyte of j, C[1] is highbyte of j
};
uint8_t T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
// T[16]中的16是2+14，2為start btye 255 255，14為要傳7個參數到電腦，拆成high byte和low byte，所以是7*2=14
// 若要更改傳到電腦的參數個數，則改x*2=y，x為要傳的參數個數，y為char T[]裡面要有多少個0
int i = 0;

// function
void init_UART();
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_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();
//    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;

        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()
{
    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()
{
    uart.baud(115200);
}

void init_TIMER()
{
    timer1.attach_us(&timer1_ITR, ITR_time1);
}

void timer1_ITR()    //開始讀取資料
{
//    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;

    CS = 1;
    // 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;

    //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];
//    }
//    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()    //分割資料
{
    splitter s1;
    splitter s2;
    splitter s3;
    splitter s4;
    splitter s5;
    splitter s6;
    splitter s7;
//    splitter s8;

    s1.j = Acce_axis_data_f[0];  //  0x6161;//
    s2.j = Acce_axis_data_f[1];
    s3.j = Acce_axis_data_f[2];
    s4.j = Acce_axis_data_f[3];
    s5.j = Acce_axis_data_f[4];
    s6.j = Acce_axis_data_f[5];
    s7.j = emg_value_f;
//    s8.j = 1; // 若要傳的參數不足8個，則隨意打一個常數即可

    T[2] = Acce_axis_data[0];  //0x5A;
    T[3] = Acce_axis_data[0] >>8;
    T[4] = s2.C[0];
    T[5] = s2.C[1];
    T[6] = s3.C[0];
    T[7] = s3.C[1];
    T[8] = s4.C[0];
    T[9] = s4.C[1];
    T[10] = s5.C[0];
    T[11] = s5.C[1];
    T[12] = s6.C[0];
    T[13] = s6.C[1];
    T[14] = s7.C[0];
    T[15] = s7.C[1];





//    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) {
//            uart.putc(T[i]);
//            i++;
//        }
//        if (i >= sizeof(T)) {
//            i = 0;
//            break;
//        }
//    }
}

float lpf(float input, float output_old, float frequency)
{
    float output = 0;

    output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);

    return output;
}

//有估測器有delay，丟資料給機器學習時就不是及時動作，所以之後決定直接丟資料