File content as of revision 7:f5c5cf499311:

// test avec RTOS
#define viti 0    // 1 montage viti ;  0 montage sur table
#include "mbed.h"
#include "LSM9DS1.h"



#include "TB6549.h"

#define dt0 0.01            // pas d'integration
#define FDC_PLUS 0.9       // fin de course +
#define FDC_MOINS 0.1      // din de course -
#define ZM 2               // zone morte +/- zm
#define SP 0.5             // pwm moteur en % (0-1)


DigitalOut Led0(LED1);

Serial pc(SERIAL_TX, SERIAL_RX,115200);

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

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  ;         //calcul de RC
double a0;   //calcul du coefficient a du filtre
double b0;    //calcul du coefficient b du filtre
double a1;           //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 dt=dt0 ; // pas reel
double angle_final;
//
void calcul(void)
{
    while(1) {
        ana=0.3;
        Fc=0.05;
        RC=1./(Fc*2*pi);            //calcul de RC
        a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
        b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
        a1=a0*RC*1.0;           //calcul du coefficient a du filtre
        dt=tt.read_us()/1000000.0;
        tt.reset();
        DOF.readAccel();
        DOF.readGyro();
#if viti
        ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site
#else
        ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
#endif

        // 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;
#if viti
        gyroy=DOF.gz-gz_off;  //sur site
#else
        gyroy=-DOF.gx-gx_off;  //sur table
#endif

        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.0083);
    }
}

int main()
{

    dt=dt0;
    tt.start();
    wait(1);
    //DOF.calibration();
    DOF.begin();
    wait(1);
    DOF.calibration();
    wait(1);
    tt.reset();
    thread.start(calcul);
    

    while(1) {


        ana=0.6;
        //moteur
        float x=verin.read();
        float s=0.0;
        /*
        // sur table
        if((angle_final>2)&&(x>0.1)) s=0.6;
        else if((angle_final<-2)&&(x<0.9)) s=-0.6;
        else motor.speed(0.0);
        */
        //sur site
        if((angle_final<-ZM)&&(x>FDC_MOINS)) s=SP;
        else if((angle_final>ZM)&&(x<FDC_PLUS)) s=-SP;
        else motor.speed(0.0);
        //
        motor.speed(s);
        pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s,dt);
        ana=0.0;
        wait(0.095);

    }
}