File content as of revision 2:f3043132a959:

#include "mbed.h"
#include "rtos.h"
#include "arm_math.h" 
#include "kalman.c"
#include "LSM303DLHC.h"
#include "L3GD20H.h"

#define Rad2Dree       57.295779513082320876798154814105f

#define PID_L_KP        0.0275f              /* Proporcional */ //0.015f
#define PID_L_KI        0.000125f              /* Integral */
#define PID_L_KD        0.075f              /* Derivative */

#define PID_R_KP       0.0275f              /* Proporcional */ //0.015f
#define PID_R_KI       0.000125f              /* Integral */
#define PID_R_KD       0.075f              /* Derivative */

#define L_SP            PI/2
#define R_SP           PI/2

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalIn sw2(SW2);
DigitalIn sw3(SW3);
Serial pc(USBTX, USBRX);

LSM303DLHC imuL(D14,D15);
L3GD20H gyroL(D14,D15);

LSM303DLHC imuR(PTC11,PTC10);
L3GD20H gyroR(PTC11,PTC10);

Timer GlobalTime;
Timer ProgramTimer;

kalman filter_pitch_L; 
kalman filter_roll_L;

kalman filter_pitch_R; 
kalman filter_roll_R;

int accL[3];
int magL[3];
short gyrL[3];

int accR[3];
int magR[3];
short gyrR[3];

struct vector {
  float x;
  float y;
  float z;
} AccL, GyrL, AccR, GyrR;

float RL, RR;
double angleL[3];
double angleR[3];

float L_error;
float R_error;

float L;
float R;

unsigned long timer;
long loopStartTime;

void AccRaw2GL(int acc[3]);
void GyrRaw2DL(short gyr[3]);
void AccRaw2GR(int acc[3]);
void GyrRaw2DR(short gyr[3]);

int main() {
    arm_pid_instance_f32 L_PID;
    arm_pid_instance_f32 R_PID;
  
    //Left
    L_PID.Kp = PID_L_KP;        /* Proporcional */
    L_PID.Ki = PID_L_KI;        /* Integral */
    L_PID.Kd = PID_L_KD;        /* Derivative */
    
    //Right
    R_PID.Kp = PID_R_KP;        /* Proporcional */
    R_PID.Ki = PID_R_KI;        /* Integral */
    R_PID.Kd = PID_R_KD;        /* Derivative */
    
    arm_pid_init_f32(&L_PID, 1);
    arm_pid_init_f32(&R_PID, 1);
    
    GlobalTime.start();
    imuL.init();
    imuR.init();
    led_green = 1;
    led_red = 1;
    pc.baud(115200);
    pc.printf("Hello World from FRDM-K64F board.\r\n");
    kalman_init(&filter_pitch_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    kalman_init(&filter_pitch_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;

    while (true) {
        imuL.readAcc(accL);
        AccRaw2GL(accL);
        gyroL.read(gyrL);
        GyrRaw2DL(gyrL);
        
        imuR.readAcc(accR);
        AccRaw2GR(accR);
        gyroR.read(gyrR);
        GyrRaw2DR(gyrR);
        
        RL = sqrt(std::pow(AccL.x, 2) + std::pow(AccL.y, 2) + std::pow(AccL.z, 2));
        RR = sqrt(std::pow(AccR.x, 2) + std::pow(AccR.y, 2) + std::pow(AccR.z, 2));
        
        kalman_predict(&filter_pitch_L, GyrL.x,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch_L, acos(AccL.x/RL));
        kalman_predict(&filter_roll_L, GyrL.y,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll_L, acos(AccL.y/RL));
        
        kalman_predict(&filter_pitch_R, GyrR.x,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch_R, acos(AccR.x/RR));
        kalman_predict(&filter_roll_R, GyrR.y,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll_R, acos(AccR.y/RR));
        
        angleL[0] = kalman_get_angle(&filter_pitch_L);
        angleL[1] = kalman_get_angle(&filter_roll_L);
        
        angleR[0] = kalman_get_angle(&filter_pitch_R);
        angleR[1] = kalman_get_angle(&filter_roll_R);
        
        L_error = angleL[0] - L_SP;
        R_error = angleR[0] - R_SP;
        
        L = arm_pid_f32(&L_PID, L_error);
        R = arm_pid_f32(&R_PID, R_error);
        
        timer = ProgramTimer.read_us();
         
        printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1]);
        printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1]);
        wait(0.2);
    }
}

void AccRaw2GL(int acc[3]){
    AccL.x = acc[0]/1024.0f; 
    AccL.y = acc[1]/1024.0f; 
    AccL.z = acc[2]/1024.0f;
}

void GyrRaw2DL(short gyr[3]){
    GyrL.x = gyr[0]/225.0f; 
    GyrL.y = gyr[1]/225.0f; 
    GyrL.z = gyr[2]/225.0f;
}

void AccRaw2GR(int acc[3]){
    AccR.x = acc[0]/1024.0f; 
    AccR.y = acc[1]/1024.0f; 
    AccR.z = acc[2]/1024.0f;
}

void GyrRaw2DR(short gyr[3]){
    GyrR.x = gyr[0]/225.0f; 
    GyrR.y = gyr[1]/225.0f; 
    GyrR.z = gyr[2]/225.0f;
}