File content as of revision 2:952d41c26b43:

// test avec RTOS
#include "mbed.h"
#include "LSM9DS1.h"



#include "TB6549.h"

#define dt 0.01     // pas d'integration

DigitalOut Led0(LED1);

Serial pc(SERIAL_TX, SERIAL_RX,115200);
Ticker calc;
Ticker affich;
LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);


AnalogIn verin(PC_3);  // lecture pos verin
AnalogOut ana (PA_5);   // pour debug analogique ISR
Motor motor(PB_4,PA_1,PA_4,PC_7);
int flag_affich=0;
int flag_imu=0;
double pi= 3.1415926535897932;
double gx_off=0,gy_off=0,gz_off=0;
double ang_off=0;


//void gyro_zero(void);
//void angle_zero(void);

void calcule()
{
    flag_imu=1;
}
void affiche()
{
    flag_affich=1;
}

int main()
{

    double ang;
    // fitres complémentaires
    double Fc=0.05;
    double RC=1./(Fc*2*pi);            //calcul de RC
    double a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
    double b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
    double a1=a0*RC*1.0;           //calcul du coefficient a du filtre

    double angle_acce_pred=0.0f;
    double angle_acce=0.0f;
    double angle_acce_f_pred=0.0f;
    double angle_acce_f=0.0f;
    //
    double gyroy_pred=0.0f;
    double gyroy=0.0f;
    double angle_gyroy_f_pred=0.0f;
    double angle_gyroy_f=0.0f;
    //
    double angle_final;
    wait(1);
    //DOF.calibration();
    DOF.begin();
    wait(1);
    DOF.calibration();
    wait(1);
    calc.attach(&calcule,dt);
    affich.attach(&affiche,0.1);
    while(1) {
        if(flag_imu) {
            ana=0.6;
            DOF.readAccel();
            DOF.readGyro();
            ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
            //ang=((180/pi)*atan2((double)DOF.ax,(double)DOF.ay)-ang_off);  // sur site

            // filtres complémentaires
            angle_acce_pred = angle_acce;
            angle_acce=ang;
            angle_acce_f_pred = angle_acce_f;
            angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred;      //filtrage accéléromètre

            gyroy_pred = gyroy;
            gyroy=-DOF.gx-gx_off;  //sur table
            // gyroy=-DOF.gz-gz_off;  //sur site
            angle_gyroy_f_pred = angle_gyroy_f;
            angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
            //
            angle_final=angle_acce_f+angle_gyroy_f;
            //
            //
            flag_imu=0;
            ana=0.0;
        }
        if(flag_affich) {
            ana=0.3;
            //moteur
            float x=verin.read();
            float s=0.0;
            if((angle_final>2)&&(x>0.1)) s=0.4;
            else if((angle_final<-2)&&(x<0.9)) s=-0.4;
            else motor.speed(0.0);
            motor.speed(s);
            pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s);
            ana=0.0;
            flag_affich=0;
        }
    }
}

void gyro_zero(void)
{
    const int NN=1000;
    //float GyroBuffer[3];
    for(int i=0; i<NN; i++) {
        DOF.readGyro();
        gx_off=gx_off+DOF.gx/(NN);
        gy_off=gy_off+DOF.gy/(NN);
        gz_off=gz_off+DOF.gz/(NN);
    }
}
void angle_zero(void)
{
    const int NN=1000;
    //int16_t AccBuffer[3];
    for(int i=0; i<NN; i++) {
        DOF.readAccel();
        double ang=(180/pi)*atan2((double)DOF.ay,(double)DOF.az);  // sur table
        //double ang=(180/pi)*atan2((double)DOF.ax,(double)DOF.ay);  // sur site
        ang_off=ang_off+ang/NN;
    }
}