File content as of revision 3:874424bbe577:

#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.0f                 /* Integral */
#define PID_L_KD       0.0f                  /* Derivative */

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

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

#define YOFF  100.0    // Y-Offset
#define XOFF  2.0      // X-Offset
#define WOFF  12.0     // Winkel-Offset

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);

PwmOut M1(D13);
PwmOut M2(D12);
DigitalOut M1D(D11);
DigitalOut M2D(D10);

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]);
double calcHeading(int *mag);

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();
    M1.period(1.0f/1000.0f);               //Comparten el mismo timer
    M1.pulsewidth(0.0f/1000.0f);
    M2.pulsewidth(0.0f/1000.0f);
    
    led_green = 1;
    led_red = 1;
    M1D = 0;
    M2D = 0;
    pc.baud(115200);
    
    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);
        imuL.readMag(magL);
        AccRaw2GL(accL);
        gyroL.read(gyrL);
        GyrRaw2DL(gyrL);
        
        imuR.readAcc(accR);
        imuR.readMag(magR);
        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);
        
        angleL[2] = calcHeading(magL);
        angleR[2] = calcHeading(magR);
        
        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\tYaw=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1],angleL[2]);
        //printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1],angleR[2]);
        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;
}

double calcHeading(int *mag)
{
    double x,y;
    double hdg;
    
    x = mag[0] + XOFF;                  // X-Achsen Ausgleich
    y = mag[1] + YOFF;                  // Y-Achsen Ausgleich
    
    hdg = atan(y/x);
    hdg *= Rad2Dree;                     // Umrechnung von Bogen- nach Winkelmass
    if (x > 0)
    {
        if(y>0) hdg = hdg;              // Korrektur 1. Quadrant
        else    hdg = 360.0 + hdg;      // Korrektur 4. Quadrant
    }
    else
    {   
        if(y>0) hdg = 180.0 + hdg;      // Korrektur 2. Quadrant
        else hdg = 180.0 + hdg;         // Korrektur 3. Quadrant
    }
    
    hdg -= WOFF;                        // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
    if (hdg <0) hdg = 360.0 + hdg;        // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
    return (hdg);
}