File content as of revision 4:b9dd320947ff:

#include "mbed.h"
#include "LSM9DS0_SH.h"

#define pi 3.141592f
#define d2r 0.01745329f

#define Rms 5000            //TT rate
#define dt  0.003f
#define NN  200

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~structure~~~//
DigitalOut  led(D13);           //detection
DigitalOut  TT_ext(D12);

//~~~IMU_SPI~~~//
DigitalOut  SPI_CSG(D7,1);      //low for GYRO enable
DigitalOut  SPI_CSXM(D6,1);     //low for ACC/MAG enable
SPI spi(D4, D5, D3);            //MOSI MISO SCLK

//~~~Servo out~~~//
PwmOut      Drive1(D8);         //control leg
PwmOut      Drive2(D9);
PwmOut      Drive3(D10);
PwmOut      Drive4(D11);
PwmOut      Drive5(D14);
PwmOut      Drive6(D15);

//~~~Serial~~~//
Serial      pc(D1, D0);         //Serial reg(TX RX)
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~globle~~~//
Ticker  TT;                         //call a timer
int     count = 0;                  //one second counter for extrenal led blink

//~~~PWMreference~~~//
const int   pwm_mid = 1450;         //+2080 for minimall lenght
const int   PWM_base[1][6] = {      //desired six reference command at 0 deg
    {pwm_mid+70,pwm_mid+10,pwm_mid+40,pwm_mid+60,pwm_mid+10,pwm_mid+30},
//    {pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid},      //only for debugging
};
int   PWM[1][6] = {                 //desired six reference command by PWM_base + PWM
    {0,0,0,0,0,0},                  //transfer equaliyu 10us to 1 deg
};

//~~~RR varible~~~//
float       Z0 = 2.45;                  //as mid point hight
float       Z_dis = -0.087;             //move rotation center

//~~~stPF_lenth_uni varible~~~//
const float alpha = 2.094f;             //pair angle
const float beta = 0.1309f;             //couple angle
const float   A[3][6] = {                   //base cood
    {cos(0.5f*alpha-beta),cos(0.5f*alpha+beta),cos(1.5f*alpha-beta),cos(1.5f*alpha+beta),cos(2.5f*alpha-beta),cos(2.5f*alpha+beta)},
    {sin(0.5f*alpha-beta),sin(0.5f*alpha+beta),sin(1.5f*alpha-beta),sin(1.5f*alpha+beta),sin(2.5f*alpha-beta),sin(2.5f*alpha+beta)},
    {0,0,0,0,0,0}
};
const float   B[3][6] = {                   //top cood(static)
    {cos(beta),cos(alpha-beta),cos(alpha+beta),cos(2*alpha-beta),cos(2*alpha+beta),cos(3*alpha-beta)},
    {sin(beta),sin(alpha-beta),sin(alpha+beta),sin(2*alpha-beta),sin(2*alpha+beta),sin(3*alpha-beta)},
    {Z_dis,Z_dis,Z_dis,Z_dis,Z_dis,Z_dis}
};
float   C[3][6] = {                         //top cood(active)
    {0,0,0,0,0,0},
    {0,0,0,0,0,0},
    {0,0,0,0,0,0}
};
float   VEC[1][6] = {                       //desired six reference command
    {0,0,2.52,0,0,0},                       //X Y Z VEC[0][3] VEC[0][4] VEC[0][5]
};
float   L[1][6] = {                         //desired six reference command
    {0,0,0,0,0,0},
};
float   Rtot[3][3] = {                      //RR'
    {0,0,0},
    {0,0,0},
    {0,0,0}
};

//~~~stPF_tracle_R varible~~~//
const float   L90 = 2.422;      //L under 90° (195 mm)
const float   Larm = 0.4969;    //arm lenth   (40 mm) b
const float   Llink = 0.8944;   //link lenth  (72 mm)
float A_R = 0;
float B_R = 0;
float C_R = 0;

//~~~IMU_SPI~~~//
short low_byte = 0x00;              //buffer
short high_byte = 0x00;
short Buff = 0x00;
float Wx = 0.0;
float Wy = 0.0;
float Wz = 0.0;
float Ax = 0.0;
float Ay = 0.0;
float Az = 0.0;
float gDIR[1][3] = {                //g vector's direction , required unitconstrain
    {0.0,0.0,0.0},                  //X Y Z
};
float Bet = 0.005;                  //confidence of Acc data
float gUnity = 0.0;
float Gdx = 0.0;
float Gdy = 0.0;
float Gdz = 0.0;
float Ele_phy = 0.0;                //estimation of top plate attitide
float Til_phy = 0.0;
float Ele_phy_int = 0.0;
float Til_phy_int = 0.0;
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void    init_TIMER();           //set TT_main() rate
void    TT_main();              //timebase function rated by TT
void    init_IO();              //initialize IO state
void    init_IMU();             //initialize IMU
void    init_Gdrift();          //read Gdrift at start up
void    read_IMU();             //read IMU data give raw data
void    state_update();         //estimation of new attitude

void    RR();                   //status generator
void    stPF_lenth_uni();       //referenve generator
void    stPF_travle_R();        //lenth to pwm pulse width

float   lpf(float input, float output_old, float frequency);    //lpf discrete
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
int main()
{
    pc.baud(115200);            //set baud rate
    wait_ms(1000);

    init_IO();                  //initialized value
    init_IMU();                 //initialize IMU
    init_Gdrift();              //read Gdrift at start up
    wait_ms(1000);

    init_TIMER();               //start TT_main

    while(1) {                  //main() loop
        if(count >= NN) {       //check if main working
            count=0;
            led = !led;
        }
    }

}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_TIMER()                   //set TT_main{} rate
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                      //interrupt function by TT
{
    TT_ext = !TT_ext;               //indicate TT_main() function working
    count = count+1;                //one second counter

    read_IMU();                     //read IMU data give raw data
    state_update();                 //estimation of new attitude

    RR();                           //VEC generated
    stPF_lenth_uni();               //L generated
    stPF_travle_R();                //PWM generated

    for(int i=0; i<6; i++) {            //safty constrain
        PWM[0][i] =  constrain(PWM[0][i],725,2025);
    }

    Drive1.pulsewidth_us(PWM[0][0]);    //drive command
    Drive2.pulsewidth_us(PWM[0][1]);
    Drive3.pulsewidth_us(PWM[0][2]);
    Drive4.pulsewidth_us(PWM[0][3]);
    Drive5.pulsewidth_us(PWM[0][4]);
    Drive6.pulsewidth_us(PWM[0][5]);

//for Serial-Oscilloscope
//    pc.printf("%.3f\r", Bet);
    pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy);
//    pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]);
//    pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
//    pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IO(void)                  //initialize
{
    TT_ext = 0;
    led = 1;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IMU(void)                 //initialize
{
    //gloable config
    SPI_CSXM = 1;                   //high as init for disable SPI
    SPI_CSG = 1;
    spi.format(8, 0);               //byte width, spi mode
    spi.frequency(4000000);         //8MHz

    //for GYRO config
    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG1_G);
    spi.write(0x9F);                //data rate 380 Hz/ cut off 25 Hz
    SPI_CSG = 1;                    //end spi talking

    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG4_G);
    spi.write(0x10);                //Scle 500dps
    SPI_CSG = 1;                    //end spi talking

    //for ACC config
    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG1_XM);
    spi.write(0x87);                //data rate 400 Hz/ Enable
    SPI_CSXM = 1;                   //end spi talking

    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG2_XM);
    spi.write(0xC8);                //cut off 50 Hz/ Scale +-4g
    SPI_CSXM = 1;                   //end spi talking
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_Gdrift(void)              //initialize
{
    for(int i=0; i<1000; i++) {
        read_IMU();                 //note Gdrift = 0 at this moment
        gDIR[0][0] = gDIR[0][0] - Wx;
        gDIR[0][1] = gDIR[0][1] - Wy;
        gDIR[0][2] = gDIR[0][2] - Wz;
        wait_ms(2);
    }
    Gdx = gDIR[0][0] /1000.0f;
    Gdy = gDIR[0][1] /1000.0f;
    Gdz = gDIR[0][2] /1000.0f;
//    pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);
    gDIR[0][0] = 0;
    gDIR[0][1] = 0;
    gDIR[0][2] = -1;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void read_IMU(void)                 //read IMU data give raw data
{
    //Wx
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wx = Buff * Gpx + Gdx;
    Wx = lpf(Buff * Gpx + Gdx, Wx, 18.0f);
    //Wy
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wy = Buff * Gpy + Gdy;
    Wy = lpf(Buff * Gpy + Gdy, Wy, 18.0f);
    //Wz
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wz = Buff * Gpz + Gdz;
    Wz = lpf(Buff * Gpz + Gdz, Wz, 18.0f);
    //Ax
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ax = lpf(Buff*Apx + Adx, Ax, 15.0f);
    //Ay
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ay = lpf(Buff*Apy + Ady, Ay, 15.0f);
    //Az
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Az = lpf(Buff*Apz + Adz, Az, 15.0f);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void state_update(void)         //estimation of new attitude
{
    //pridict
    gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
    gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
    gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;

    //update
    gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax;
    gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay;
    gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az;

    //nutralizing
    gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] );
    for(int i=0; i<3; i++) {
        gDIR[0][i] =  gDIR[0][i] / gUnity;
    }

    //transfer
    Ele_phy = asin(gDIR[0][0]);
    Til_phy = asin(-gDIR[0][1] / cos(Ele_phy));
//    //test
//    gDIR[0][0] = gDIR[0][0] + Wx*dt;
//    gDIR[0][1] = gDIR[0][1] + Wy*dt;
//    gDIR[0][2] = gDIR[0][2] + Wz*dt;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void RR()                           //status generator
{
    Ele_phy_int = Ele_phy_int + Ele_phy*dt;
    Til_phy_int = Til_phy_int + Til_phy*dt;
    Ele_phy_int =  constrain(Ele_phy_int,-0.01f,0.01f);
    Til_phy_int =  constrain(Til_phy_int,-0.01f,0.01f);

    VEC[0][4] = -9.5f*Ele_phy - 0.17f*Wy - 60.0f*Ele_phy_int;
    VEC[0][5] = -9.5f*Til_phy - 0.17f*Wx - 60.0f*Til_phy_int;

    VEC[0][4] =  constrain(VEC[0][4],-0.30f,0.30f);
    VEC[0][5] =  constrain(VEC[0][5],-0.30f,0.30f);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void stPF_lenth_uni()               //referenve generator
{
    Rtot[0][0] =    cos(VEC[0][4])*cos(VEC[0][3]);
    Rtot[0][1] =  - cos(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][3])*sin(VEC[0][4])*sin(VEC[0][5]);
    Rtot[0][2] =    sin(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][5])*cos(VEC[0][3])*sin(VEC[0][4]);
    Rtot[1][0] =    cos(VEC[0][4])*sin(VEC[0][3]);
    Rtot[1][1] =    cos(VEC[0][5])*cos(VEC[0][3]) + sin(VEC[0][4])*sin(VEC[0][5])*sin(VEC[0][3]);
    Rtot[1][2] =  - cos(VEC[0][3])*sin(VEC[0][5]) + cos(VEC[0][5])*sin(VEC[0][4])*sin(VEC[0][3]);
    Rtot[2][0] =  - sin(VEC[0][4]);
    Rtot[2][1] =    cos(VEC[0][4])*sin(VEC[0][5]);
    Rtot[2][2] =    cos(VEC[0][4])*cos(VEC[0][5]);

    for(int j=0; j<6; j++) {                    //reset C
        for(int i=0; i<3; i++) {
            C[i][j] = 0;
        }
    }

    for(int i=0; i<6; i++) {                    //(RR.' * B)
        for(int j=0; j<3; j++) {
            for(int k=0; k<3; k++) {
                C[j][i] = Rtot[j][k] * B[k][i] + C[j][i];
            }
        }
    }

    for(int j=0; j<6; j++) {                    //+ [X,Y,Z]
        for(int i=0; i<3; i++) {
            C[i][j] = C[i][j] + VEC[0][i];
        }
    }
    for(int i=0; i<6; i++) {
        float X = C[0][i]-A[0][i];
        float Y = C[1][i]-A[1][i];
        float Z = C[2][i]-A[2][i];
        L[0][i] = sqrt(X*X+Y*Y+Z*Z);
    }
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void stPF_travle_R()                //lenth to pwm pulse width
{
    for(int i=0; i<6; i++) {
        A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm;
        B_R = Llink/Larm;
        C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2);
        PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i];
    }
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
float lpf(float input, float output_old, float frequency)
{
    float output = 0;
    output = (output_old + frequency*dt*input) / (1 + frequency*dt);
    return output;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//