File content as of revision 11:9d83bb9a611c:

// test avec RTOS
#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               // zone morte +/- zm
#define SP 0.5             // pwm moteur en % (0-1)

void calcul(void);
DigitalOut Led0(LED1);

Serial pc(SERIAL_TX, SERIAL_RX,115200);

LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
Thread thread (osPriorityAboveNormal);
EventQueue queue;
Mutex pr;
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);
Ticker tic;

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;
//
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 angle_final;
//
void calcul(void)
{
    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
    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;
}
//
int main()
{
    wait(1);
    DOF.begin();
    wait(1);
    DOF.calibration();
    wait(1);
    thread.start(callback(&queue,&EventQueue::dispatch_forever));
    tic.attach(queue.event(&calcul),dt);
    while(1) {
        ana=0.6;
        //moteur
        float x=verin.read();
        float s=0.0;
        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);
        //pr.lock();
        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);
        //pr.unlock();
        ana=0.0;
        wait(0.095);
    }
}