File content as of revision 7:d421b158f1be:

#include "mbed.h"
#include "LSM9DS1.h"
#include "AS5145.h"
#include "math.h"

LSM9DS1 imu2(D5, D7);
LSM9DS1 imu3(D3, D6);
LSM9DS1 imu(D14, D15);
//AnalogIn sEMG(D13);
//AnalogIn sEMG2(D1);
//AnalogIn sEMG3(D0);
//AnalogIn sEMG4(PC_4);
Serial uart_1(USBTX, USBRX);
//Serial uart_1(D10,D2);            // TX : D10     RX : D2           // serial 1
AnalogIn FSR(A0);
AnalogIn EMG(A1);

/********************************************************************/
// timer
/********************************************************************/
#define LOOPTIME  1000                         // 1 ms
Ticker timer1;
Timer t;
float T = 0.001;

/********************************************************************/
//function declaration
/********************************************************************/
void init_TIMER(void);
void timer1_interrupt(void);
//void init_uart(void);
void separate(void);
void uart_send(void);
void setup(void);
void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha);
float lpf(float input, float output_old, float frequency);
void angle_fn(float x1_hat[3],float x2_hat[3]);
void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]);
void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]);

/********************************************************************/
// uart send data
/********************************************************************/
int display[20];
char onebytedata[42] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};      // char onebytedata[0] , onebytedata[1] : 2 start byte
unsigned long time_flag;
unsigned long uart_flag;
unsigned long Tstart;
unsigned long Tperiod;

/********************************************************************/
// FSR
/********************************************************************/
float Voltage_FSR;

/********************************************************************/
// EMG
/********************************************************************/
//sEMG variable
//float emg_value[4] = {0.0f};
float Voltage_EMG = 0.0f;


/********************************************************************/
// imu data
/********************************************************************/
int16_t Gyro_axis_data[9] = {0};     // X, Y, Z axis
int16_t Acce_axis_data[9] = {0};     // X, Y, Z axis
float Gyro_axis_data_f[9] = {0};
float Gyro_axis_data_f_old[9] = {0};
float Acce_axis_data_f[9] = {0};
float Acce_axis_data_f_old[9] = {0};
float axm[3] = {0.0f};
float aym[3] = {0.0f};
float azm[3] = {0.0f};
float w1[3] = {0.0f};
float w2[3] = {0.0f};
float w3[3] = {0.0f};
//estimator
float x1_hat[3] = {0.0f};
float x2_hat[3] = {0.0f};
float sinroll[3] = {0.0f};
float cosroll[3] = {0.0f};
float sinpitch[3] = {0.0f};
float pitch_angle[3] = {0.0f};
float roll_angle[3] = {0.0f};
float yaw_dot[3] = {0.0f};
float pitch_dot[3] = {0.0f};
float pitch_double_dot[3] = {0.0f};
float pitch_double_dot_f[3] = {0.0f};
float pitch_double_dot_f_old[3] = {0.0f};
float pitch_dot_old[3] = {0.0f};
float axm_f[3] = {0.0f};
float axm_f_old[3] = {0.0f};
float w3aym_f[3] = {0.0f};
float w3aym_f_old[3] = {0.0f};
float w2azm_f[3] = {0.0f};
float w2azm_f_old[3] = {0.0f};
float aym_f[3] = {0.0f};
float aym_f_old[3] = {0.0f};
float w3axm_f[3] = {0.0f};
float w3axm_f_old[3] = {0.0f};
float w1azm_f[3] = {0.0f};
float w1azm_f_old[3] = {0.0f};
float azm_f[3] = {0.0f};
float azm_f_old[3] = {0.0f};
float w2axm_f[3] = {0.0f};
float w2axm_f_old[3] = {0.0f};
float w1aym_f[3] = {0.0f};
float w1aym_f_old[3] = {0.0f};

/**********************************************************************************************************************************/
int main()
{
    //Start uart
    t.start();
    uart_flag = 0;
    Tperiod = 0;
    uart_1.baud(115200);
    
    setup();  //Setup sensors
    AS5145_begin(); //begin encoder
    init_TIMER();
    
    while (1)
    {
        uart_send();
        //uart_1.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096);
        wait(0.05);
        //uart_1.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]);
        //uart_1.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096);
        //uart_1.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]);
        //uart_1.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]);
        //uart_1.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]);
        //uart_1.printf("position: %d,%d\r\n", position[0], position[1]);
        //uart_1.printf("roll_angle: %2f\r\n",roll_angle);
        //uart_1.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2);
    }
}

void setup()
{
    imu.begin();
    imu2.begin();
    imu3.begin();
}

/********************************************************************/
// init_TIMER
/********************************************************************/
void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
    timer1.attach_us(&ReadValue, 10000);//10ms interrupt period (100 Hz)
}

/********************************************************************/
// uart_send
/********************************************************************/
int i = 0;  //count one byte data
void uart_send(void)
{
    display[0] = ;
    display[1] = ;
    display[2] = ;
    display[3] = ;
    display[4] = ;
    display[5] = ;
    display[6] = ;
    display[7] = ;
    display[8] = ;
    display[9] = ;
    display[10] = ;
    display[11] = ;
    display[12] = ;
    display[13] = ;
    display[14] = ;
    display[15] = ;
    display[16] = ;
    display[17] = ;
    display[18] = Voltage_FSR;
    display[19] = Voltage_EMG;

//    wait(0.2);
    separate();

    int j = 0;
    int k = 1;
    while(k) {
        if(uart_1.writeable()) {
            uart_1.putc(onebytedata[i]);
            i++;
            j++;
        }

        if(j>1) {                   // send 2 bytes
            k = 0;
            j = 0;
        }
    }

    if(i>42)
        i = 0;
}

/********************************************************************/
// seperate
/********************************************************************/
void separate(void)
{
    int count = 2;
    for(int i = 0; i < 20; i++)
    {
        onebytedata[count] = display[i] & 0x00ff;
        onebytedata[count + 1] = display[i] >> 8;
        count = count + 2;
    }
    
//    onebytedata[2] = display[0] & 0x00ff;           // LSB(8bit)資料存入陣列
//    onebytedata[3] = display[0] >> 8;               // HSB(8bit)資料存入陣列
//    onebytedata[4] = display[1] & 0x00ff;
//    onebytedata[5] = display[1] >> 8;
//    onebytedata[6] = display[2] & 0x00ff;
//    onebytedata[7] = display[2] >> 8;
//    onebytedata[8] = display[3] & 0x00ff;
//    onebytedata[9] = display[3] >> 8;
//    onebytedata[10] = display[4] & 0x00ff;
//    onebytedata[11] = display[4] >> 8;
//    onebytedata[12] = display[5] & 0x00ff;
//    onebytedata[13] = display[5] >> 8;
//    onebytedata[14] = display[6] & 0x00ff;
//    onebytedata[15] = display[6] >> 8;
//    onebytedata[16] = display[7] & 0x00ff;
//    onebytedata[17] = display[7] >> 8;
//    onebytedata[18] = display[8] & 0x00ff;
//    onebytedata[19] = display[8] >> 8;
//    onebytedata[20] = display[9] & 0x00ff;
//    onebytedata[21] = display[9] >> 8;
//    onebytedata[22] = display[10] & 0x00ff;
//    onebytedata[23] = display[10] >> 8;
//    onebytedata[24] = display[11] & 0x00ff;
//    onebytedata[25] = display[11] >> 8;
//    onebytedata[26] = display[12] & 0x00ff;
//    onebytedata[27] = display[12] >> 8;
//    onebytedata[28] = display[13] & 0x00ff;
//    onebytedata[29] = display[13] >> 8;
//    onebytedata[30] = display[14] & 0x00ff;
//    onebytedata[31] = display[14] >> 8;
//    onebytedata[32] = display[15] & 0x00ff;
//    onebytedata[33] = display[15] >> 8;
//    onebytedata[34] = display[16] & 0x00ff;
//    onebytedata[35] = display[16] >> 8;
//    onebytedata[36] = display[17] & 0x00ff;
//    onebytedata[37] = display[17] >> 8;
//    onebytedata[38] = display[18] & 0x00ff;
//    onebytedata[39] = display[18] >> 8;
//    onebytedata[40] = display[19] & 0x00ff;
//    onebytedata[41] = display[19] >> 8;
}

/********************************************************************/
// timer1_interrupt
/********************************************************************/
void timer1_interrupt(void)
{  
    int i;
    imu.readAccel();
    imu.readGyro();
    imu2.readAccel();
    imu2.readGyro();
    imu3.readAccel();
    imu3.readGyro();
    // sensor raw data 定義座標
    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;
    Acce_axis_data[6] = -imu3.ax*Acce_gain_x_2;
    Acce_axis_data[7] = -imu3.az*Acce_gain_y_2;
    Acce_axis_data[8] = -imu3.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;
    Gyro_axis_data[6] = -imu3.gx*Gyro_gain_x_2;
    Gyro_axis_data[7] = -imu3.gz*Gyro_gain_y_2;
    Gyro_axis_data[8] = -imu3.gy*Gyro_gain_z_2;
    
    for(i=0;i<9;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];
    }
    
    axm[0] = Acce_axis_data_f[0];  //第一顆imu
    aym[0] = Acce_axis_data_f[1];
    azm[0] = Acce_axis_data_f[2];
    w1[0]  = Gyro_axis_data_f[0];
    w2[0]  = Gyro_axis_data_f[1];
    w3[0]  = Gyro_axis_data_f[2];
    axm[1] = Acce_axis_data_f[3];  //第二顆imu
    aym[1] = Acce_axis_data_f[4];
    azm[1] = Acce_axis_data_f[5];
    w1[1]  = Gyro_axis_data_f[3];
    w2[1]  = Gyro_axis_data_f[4];
    w3[1]  = Gyro_axis_data_f[5];
    axm[2] = Acce_axis_data_f[6];  //第三顆imu
    aym[2] = Acce_axis_data_f[7];
    azm[2] = Acce_axis_data_f[8];
    w1[2]  = Gyro_axis_data_f[6];
    w2[2]  = Gyro_axis_data_f[7];
    w3[2]  = Gyro_axis_data_f[8];
    
    
    estimator(axm,aym,azm,w3,w2,w1,120);
    angle_fn(x1_hat,x2_hat);
    pitch_dot_fn(w3,w2,w1,sinroll,cosroll);
    pitch_double_dot_fn(pitch_dot,pitch_dot_old); 

    for(i=0;i<3;i++)
    {
        pitch_dot_old[i] = pitch_dot[i];
    }
//    emg_value[0] = sEMG.read(); 
//    emg_value[1] = sEMG2.read(); 
//    emg_value[2] = sEMG3.read();
//    emg_value[3] = sEMG4.read();
      Voltage_FSR = FSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
      Voltage_EMG = EMG.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
      Voltage_FSR = Voltage_FSR * 33; // Change the value to be in the 0 to 3300 range
      Voltage_EMG = Voltage_EMG * 33; // Change the value to be in the 0 to 3300 range
      time_flag = t.read_ms();
}
/********************************************************************/
// estimator
/********************************************************************/
void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha)
{
    int i;
    for(i=0;i<3;i++)
    {
        axm_f[i] = lpf(axm[i],axm_f_old[i],alpha);   //axm_f濾波
        axm_f_old[i] = axm_f[i];
        w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha);   //w3aym_f濾波
        w3aym_f_old[i] = w3aym_f[i];
        w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha);    //w2azm_f濾波
        w2azm_f_old[i] = w2azm_f[i];
        aym_f[i] = lpf(aym[i],aym_f_old[i],alpha);    //aym_f濾波
        aym_f_old[i] = aym_f[i];
        w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha);    // w3axm_f濾波
        w3axm_f_old[i] = w3axm_f[i];
        w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha);    //w1azm_f濾波
        w1azm_f_old[i] = w1azm_f[i];
    
        x1_hat[i] = axm_f[i] + w3aym_f[i]/alpha - w2azm_f[i]/alpha;
        x2_hat[i] = -w3axm_f[i]/alpha + aym_f[i] + w1azm_f[i]/alpha;
    }
    
}
/********************************************************************/
// angle_fn
/********************************************************************/
void angle_fn(float x1_hat[3],float x2_hat[3])
{
    int i;
    for(i=0;i<3;i++)
    {
        sinroll[i] = x2_hat[i]*(-0.1020);
        if(sinroll[i] >= 1.0f)
        {
            sinroll[i] = 1.0;
            cosroll[i] = 0.0;
        }
        else if(sinroll[i] <= -1.0f)
        {
            sinroll[i] = -1.0;
            cosroll[i] = 0.0;
        }
        else cosroll[i] = sqrt(1-(sinroll[i]*sinroll[i]));
        roll_angle[i] = (asin(sinroll[i]))*180/pi;
        sinpitch[i] = x1_hat[i]*(0.1020f)/cosroll[i];
        if(sinpitch[i] >= 1.0f)
        {
            sinpitch[i] = 1.0;
        }
        else if(sinpitch[i] <= -1.0f)
        {
            sinpitch[i] = -1.0;
        }
    
        pitch_angle[i] = (asin(sinpitch[i]))*180/pi;
    }
}
void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3])
{
    int i;
    for(i=0;i<3;i++)
    {
        yaw_dot[i] = (w3[i]*cosroll[i] - w1[i]*sinroll[i])/cosroll[i];
        pitch_dot[i] = w2[i] - yaw_dot[i]*sinroll[i];
    }
}
void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3])
{
    int i;
    for(i=0;i<3;i++)
    {
        pitch_double_dot[i] = (pitch_dot[i] - pitch_dot_old[i])/0.01f;
        pitch_double_dot_f[i] = lpf(pitch_double_dot[i],pitch_double_dot_f_old[i],30);
        pitch_double_dot_f_old[i] = pitch_double_dot_f[i]; 
    }
}
/********************************************************************/
// lpf
/********************************************************************/
float lpf(float input, float output_old, float frequency)
{
    float output = 0;
    
    output = (output_old + frequency*T*input) / (1 + frequency*T);
    
    return output;
}