File content as of revision 4:b40027cb3012:

// 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);
Thread thread (osPriorityAboveNormal);

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);
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;
void calcul(void)
{
    while(1) {
        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;

        //
        //

        ana=0.0;
        wait(0.0082);
    }
}

int main()
{


    wait(1);
    //DOF.calibration();
    DOF.begin();
    wait(1);
    DOF.calibration();
    wait(1);
    thread.start(calcul);

    while(1) {


        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;
        wait(0.1);

    }
}

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