File content as of revision 0:561f7672eaad:

#include "mbed.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"
#include "Servo.h"
#include "math.h"
#define TS 10                    // sample time [ms]
#define SIMULATION_TIME 10        // simulation time [s]

LocalFileSystem local("local");  


//funtion declaration
void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3]);
void getEulerAngles(float angles[3], float RotMatrix[9]);
void matPrint(float *A, int r, int c);
void matSum(float *C, float *A, float *B, int r, int c);
void matProd(float *C, float *A, float *B, int r, int cA, int cB);
void matTimesVect(float *b, float *A, float *v, int r, int c);
void transpose(float *B, float *A, int n);
float norm(float *v, int r);
void hat(float *B, float *A);
void moveA();
void moveB();
void moveC();
//variable definition
float right_pectoral_freq;
float left_pectoral_freq;
float tail_freq;
bool directionA=1;
bool directionB=1;
bool directionC=1;
float speedA=1;
float speedB=1;
float speedC=1;

float gx ;
float gy ;
float gz ;
float ax ;
float ay ;
float az ;
float mx ;
float my ;
float mz ;
float GYRO[3], ACC[3], MAGN[3];
float G[3], A[3], M[3], A_0[3], M_0[3];

const float pi = 3.1415926;
const float KRG = 57.2958;                   // radians to degrees

//G offset: 0.918820 0.550812 -1.280107
//A offset: -0.001172 0.989633 0.123766
const float gx_static_offset=0.918820;
const float gy_static_offset=0.550812;
const float gz_static_offset=-1.280107;



const float gain_gyro = 0.00013315;
const float gain_acc = 0.0098;
const float gain_magn = 0.005035;
float Ka = 0.0001;                            // accelerometer estimation gain
float Kg = 0.0004;                            // gyro estimation gain
float Km = 0;                            // magnetometer estiamtion gain
float g_0[3] = {0.0, 0.0, 9.8};                // gavity vector in S
float m_0[3] = {0.0, 0.0, 0.0};
float angles[3] = {0.0, 0.0, 0.0};
float R_est[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0};
float R_old[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
float eye3[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};

float rho=0;
float pitch=0;
float yaw=0;

//sinusoidal trajectory parameters
float freq_l=3;
float freq_r=3;
float freq_t=3;

float Amp_l=0;
float Amp_r=0;
float Amp_t=0;

float center_l=0;
float center_r=0;
float center_t=0;

float phi_l=0;
float phi_r=0;
float phi_t=0;


float IR_l_data=0.0;
float IR_r_data=0.0;


Ticker tickA;
Ticker tickB;
Ticker tickC;
Ticker t3;
//Timer realTime;

//sensor declaration
LSM303DLHC compass(p28, p27);
L3GD20 gyro(p28, p27);
Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");
//actuator declaration
//PwmOut fin_l(p22);//was 23
//PwmOut fin_r(p25);//was 25
//PwmOut fin_t(p26);//was 21

AnalogIn IR_l(p19);
AnalogIn IR_r(p20);


PwmOut PWMA(p25);//RIGHT  FIN
PwmOut PWMB(p23);//LEFT FIN
PwmOut PWMC(p21);//TAIL  FIN
DigitalOut STBY1 (p30);
DigitalOut STBY2 (p29);
DigitalOut AIN1 (p8);
DigitalOut AIN2 (p11);
DigitalOut BIN1 (p7);
DigitalOut BIN2 (p6);
DigitalOut CIN1 (p16);
DigitalOut CIN2 (p17);
int inPin1;
int inPin2;

float period_pwm;
void move(int motor, float speed, int direction);
void stop();
void loop();

//time parameters
float t1=0.0;
int sample_interval_us=TS*1000; // dt=TS sec
float dt;


void loop()
{

pc.printf("loop start");
    gyro.read(&G[0],&G[1], &G[2]);
    compass.read(&A[0],&A[1],&A[2],&M[0],&M[1],&M[2]);

    G[0]=G[0] - gx_static_offset;
    G[1]=G[1] - gy_static_offset;
    G[2]=G[2] - gz_static_offset;

    //M[0]=mx;
    //M[1]=my;
    //M[2]=mz;

    A[0]=A[0];
    A[1]=A[1];
    A[2]=A[2];
    pc.printf("a0: %f %f %f  \n \r",A[0], A[1], A[2]);
    pc.printf("g0: %f %f %f  \n \r ",G[0], G[1], G[2]);
    //pc.printf("m0: %f %f %f  \n \r \n \r",mx, my, mz);
    pc.printf("\n\r");
    GYRO[0] = G[0]*gain_gyro;
    GYRO[1] = G[1]*gain_gyro;
    GYRO[2] = G[2]*gain_gyro;
    MAGN[0] = M[0]*gain_magn;
    MAGN[1] = M[1]*gain_magn;
    MAGN[2] = M[2]*gain_magn;
    ACC[0] = -A[0]*gain_acc;
    ACC[1] = -A[1]*gain_acc;
    ACC[2] = -A[2]*gain_acc;
    //pc.printf("a1: %f %f %f  \n \r",ACC[0], ACC[1], ACC[2]);
    //pc.printf("g1: %f %f %f  \n \r",GYRO[0], GYRO[1], GYRO[2]);
    // pc.printf("m1: %f %f %f  \n \r \n \r",MAGN[0], MAGN[1], MAGN[2]);
    pc.printf("\n\r");
    //matProd(R_old, R_est, eye3, 3, 3, 3);
    sensorFusion(R_est, R_old, G, A, M);
    getEulerAngles(angles, R_est);

    rho=KRG*angles[0];
    yaw=KRG*angles[1];
    pitch=KRG*angles[2];
    pc.printf("angles= %f %f %f \n\r \n \r",rho,pitch,yaw);
    //float thisTime=realTime.read();
//    FILE *fp = fopen("/local/test.txt", "w"); 
//    fprintf(fp, "angles= %f %f %f %f \n\r \n \r",rho,pitch,yaw,thisTime);
//    fclose(fp);
//controller
//time
//    t1=t1+dt;
}

int main()
{
//initialization
    pc.baud(9600);



    //realTime.start();

    //setup for PWM
    period_pwm=0.01;
    PWMA.period(period_pwm);          // servo requires a 20ms period
    PWMB.period(period_pwm);          // servo requires a 20ms period

    directionA=1;
    directionB=1;
    directionC=1;
    speedA=1;
    speedB=1;
    speedC=1;
    left_pectoral_freq=3;
    right_pectoral_freq=0;
    tail_freq=3;

    //dt=sample_interval_us/1000000.0  ;
    pc.printf("loop attach \n \r");
    t3.attach(&loop, .5);
    /*float d_freq=yaw*(.1);
    if (d_freq<0) {
        left_pectoral_freq=3-d_freq;
        right_pectoral_freq=3;
    }
    if(d_freq>0) {
        left_pectoral_freq=3;
        right_pectoral_freq=3-d_freq;
    }
    
    */
    pc.printf("after attach \n \r");
    if(left_pectoral_freq > 0) {
        float right_interval=1/right_pectoral_freq;
        tickA.attach(&moveA, right_interval);
    }

    if(right_pectoral_freq > 0) {
        float left_interval=1/left_pectoral_freq;
        tickB.attach(&moveB, left_interval);
    }

    if(tail_freq > 0) {
        float tail_interval=1/tail_freq;
        tickC.attach(&moveC, tail_interval);
    }








}

void sensorFusion(float Rest[9], float Rold[9], float gy[3], float ac[3], float ma[3])

{
    // w = gyro + a + b
    // a = KG*(acc x c)
    // b = KB*(magn x d)
    // c = RRt*gravity
    // d = RRt*mfield

    // Rest = Rold*WW
    // WW = I+II+III
    // I = eye(3)
    // II = alpha*T_c*what
    // III = beta*T_c*T_c*W2
    // W2 = what*what


    float RRt[9];
    transpose(RRt, Rold, 3);

    float a[3];
    float b[3];
    float c[3];
    float d[3];
    float acchat[9];
    float magnhat[9];
    float w[3];
    float what[9];
    float nw;
    float alpha;
    float beta;
    float II[9];
    float III[9];
    float W2[9];
    float WW[9];

    matTimesVect(c, RRt, g_0, 3, 3);
    hat(acchat, ac);
    matTimesVect(a, acchat, c, 3, 3);

    matTimesVect(d, RRt, m_0, 3, 3);
    hat(magnhat, ma);
    matTimesVect(b, magnhat, d, 3, 3);
    int k;
    for (k = 0; k < 3; k++) {
        float sum = Kg*gy[k]+(Ka*a[k])+(Km*b[k]);
        w[k] = sum;
    }
    //matPrint(w,k,1);
    hat(what, w);
    nw = norm(w, 3);
    nw = TS*nw;

    alpha = 0;
    beta = 0;

    if (nw < -0.000000001 || nw > 0.000000001) {
        alpha = (sin(nw))/nw;
        beta = (1-cos(nw))/(nw*nw);
    }

    // printf("\t %f \n", alpha);
    // printf("\t %f \n", beta);

    matProd(W2, what, what, 3, 3, 3);

    for (int j = 0; j < 9; j++) {
        II[j] = alpha*TS*what[j];
        III[j] = beta*TS*TS*W2[j];
        WW[j] = eye3[j]+II[j]+III[j];
    }

    matProd(Rest, Rold, WW, 3, 3, 3);
    //matPrint(Rest,3,3);

}


void getEulerAngles(float angles[3], float RotMatrix[9])
{
    float theta = -asin(RotMatrix[2]);
    float phi = atan2(RotMatrix[5]/cos(theta),RotMatrix[8]/cos(theta));
    float psi = atan2(RotMatrix[1]/cos(theta),RotMatrix[0]/cos(theta));
    angles[0] = phi;
    angles[1] = theta;
    angles[2] = psi;
    return;
}


// Matrix operations (matrices are stored in a one dimensional array, columnwise)
// e.g.: [1 2 3; 4 5 6; 7 8 9] = [1 4 7 2 5 8 3 6 9]


void matPrint(float *A, int r, int c)

// Print the content of the matrix A which is r x c

{
    int i, j;
    pc.printf("printing \n \r");
    for(i = 0; i < r; i++) {
        for(j = 0; j < c; j++) {
            pc.printf("%f",A[i + j*r]);
            if(j == c-1) {
                pc.printf("\n \r");
            } else {
                pc.printf("\t");
            }
        }
    }

    printf("\n");
}

void matSum(float *C, float *A, float *B, int r, int c)

//  C = A + B, where A, B and C are r x c

{
    int i, j;
    for(i = 0; i < r; i++) {
        for(j = 0; j < c; j++) {
            C[i + r*j] = A[i + r*j] + B[i + r*j];
        }
    }
    return;
}


void matProd(float *C, float *A, float *B, int r, int cA, int cB)

// C = A times B, where A is r x c, B is c x c and C is r x c

{
    float sum;
    int i, j, k;

    for(i = 0; i < r; i++) {
        for(j = 0; j < cB; j++) {
            sum = 0.0;
            for(k = 0; k < cA; k++) {
                sum = sum + A[i + k*r] * B[j*cA + k];
            }
            C[i + r*j] = sum;
        }
    }

    return;
}


void matTimesVect(float *b, float *A, float *v, int r, int c)

//   b = A times v, where A is r x c, and v and b are c-long column vectors

{
    float sum;
    int i, k;

    for(i = 0; i < r; i++) {
        sum = 0.00;
        for(k = 0; k < c; k++) {
            sum = sum + A[i + k*r] * v[k];
        }

        b[i] = sum;
    }

    return;
}


void transpose(float *B, float *A, int n)

// B = A^T, where A and B are n x n matrices

{
    int i, j;

    for(i = 0; i < n; i++) {
        for(j = 0; j < n; j++) {
            B[n*i+j] = A[i + n*j];
        }
    }

    return;
}


float norm(float *v, int r)

// 2-norm of a r-long vector

{
    float n = 0;
    int i;

    for(i = 0; i < r; i++) {
        n = n + v[i]*v[i];
    }

    n = sqrt(n);

    return n;
}


void hat(float *B, float *A)

// hat operator (for 3-D arrays only)

{
    B[0] = 0;
    B[1] = A[2];
    B[2] = -A[1];
    B[3] = -A[2];
    B[4] = 0;
    B[5] = A[0];
    B[6] = A[1];
    B[7] = -A[0];
    B[8] = 0;

    return;
}

void move(int motor, float speed, int direction)
{
//Move specific motor at speed and direction
//motor: 0 for B 1 for A
//speed: 0 is off, and 1 is full speed
//direction: 0 clockwise, 1 counter-clockwise

    STBY1=1; //disable standby
    STBY2=1; //disable standby


    if(direction == 0) {
        inPin1 = 0;
        inPin2 = 1;
    } else if(direction == 1) {

        inPin1 = 1;
        inPin2 = 0;

    }

    if(motor == 1) {
        AIN1=inPin1;
        AIN2=inPin2;
        PWMA.pulsewidth(period_pwm*speed*1.0);
    } else if(motor == 0) {
        BIN1=inPin1;
        BIN2=inPin2;
        PWMB.pulsewidth(period_pwm*speed*1.0);
    }
}


void stop()
{
//enable standby
    STBY1=0;
    STBY2=0;

}

void moveA()
{
    STBY1=1; //disable standby
    int inPin1=1;
    int inPin2=0;
    if(directionA==0) {
        inPin1 = 0;
        inPin2 = 1;
    }
    AIN1=inPin1;
    AIN2=inPin2;
    directionA=!directionA;

    //pc.printf("dirA = %d\n\r",directionA);



    PWMA.pulsewidth(period_pwm*speedA);

}
void moveB()
{
    STBY1=1; //disable standby
    int inPin1=0;
    int inPin2=1;
    if(directionB==0) {
        inPin1 = 1;
        inPin2 = 0;
    }
    //pc.printf("dirB = %d\n\r",directionB);
    BIN1=inPin1;
    BIN2=inPin2;
    directionB=!directionB;

    PWMB.pulsewidth(period_pwm*speedB);

}

void moveC()
{
    STBY1=1; //disable standby
    int inPin1=0;
    int inPin2=1;
    if(directionC==0) {
        inPin1 = 1;
        inPin2 = 0;
    }
    //pc.printf("dirC = %d\n\r",directionC);
    CIN1=inPin1;
    CIN2=inPin2;
    directionC=!directionC;

    PWMC.pulsewidth(period_pwm*speedC);

}