File content as of revision 14:f4cbc5db2873:

// *******************************************************
// HUBLEX - Université Paris Saclay - IUT de Cachan
// Version 1.0  du 3 juin 2020
// Auteur : Gilles Raynaud
//
// SteadyTech
//*********************************************************
#define viti 1    // 1 montage viti ;  0 montage sur table
#include "mbed.h"
#include "LSM9DS1.h"
#include "TB6549.h"
//
#define dt 0.01            // pas d'integration
#define FDC_PLUS 0.9       // fin de course +
#define FDC_MOINS 0.1      // din de course -
#define ZM 2.0             // zone morte +/- zm
#define SP 0.6             // pwm moteur en % (0-1)
//
void calcul(void);
//
DigitalOut LedVP(PC_8);  // led Verin P
DigitalOut LedVM(PC_5);  // led Verin M
DigitalOut LedOK(PC_6);  // led Système OK
//
Serial pc(SERIAL_TX, SERIAL_RX,115200);
//
LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
Thread thread (osPriorityAboveNormal);
EventQueue queue;
AnalogIn verin(PC_3);     // lecture position verin
AnalogOut ana (PA_5);     // pour debug analogique ISR et RTOS
Motor motor(PB_4,PA_1,PA_4,PC_7);  // commande moteur vérin
Ticker tic;
//
double pi= 3.1415926535897932;
double gx_off=0,gy_off=0,gz_off=0;
double ang_off=0;
//
double ang;
// fitres complémentaires
double Fc=0.05;  // fréquence charnière du filtre complémentaire
double RC  ;         //constante de temps
double a0;    //coefficient a0 du filtre
double b0;    //coefficient b0 du filtre
double a1;    //coefficient a1 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)
{
    ana=0.3;  // debug RTOS
    Fc=0.05;    // frequence de coupure des filtres
    RC=1./(Fc*2*pi);            //calcul de RC
    a0=1./(1+(2*RC/dt));   //calcul du coefficient a0 du filtre
    b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b0 du filtre
    a1=a0*RC*1.0;           //calcul du coefficient a1 du filtre
    DOF.readAccel();
    DOF.readGyro();
#if viti
    //ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site USB dessous
    ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
#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;
}
//
int main()
{
    LedVP=1;
    LedVM=1;
    LedOK=1;
    DOF.begin();
    for(int ii=0; ii<4; ii++) {
        DOF.calibration();
        LedVP=!LedVP;
        LedVM=!LedVM;
        LedOK=!LedOK;
    }
    LedVP=0;
    LedVM=0;
    LedOK=1;
    //wait(0.5);
    // init filtre accéléro
    DOF.readAccel();
    angle_acce_f_pred=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
    angle_acce_f=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
    //
    thread.start(callback(&queue,&EventQueue::dispatch_forever));
    tic.attach(queue.event(&calcul),dt);
    LedOK=1;
    while(1) {
        ana=0.6;  // debug RTOS
        //moteur
        double x=verin.read();
        double s=0.0;
        if((angle_final<-ZM)&&(x>FDC_MOINS)) {
            s=SP;
            LedVP=1;
            LedVM=0;
        } else if((angle_final>ZM)&&(x<FDC_PLUS)) {
            s=-SP;
            LedVP=0;
            LedVM=1;
        } else {
            s=0;
            LedVP=0;
            LedVM=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);
        //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);
        ana=0.0;
        wait(0.095);
    }
}