File content as of revision 1:1d8f9c73cdc4:

#include "PIDboussole.h"

#define T 0.03
#define Bkp 2
#define Bki 0
#define Bkd 0
#define ANGLE_TOLERANCE 5

PID_boussole::PID_boussole(Motor* _moteur_d, Motor* _moteur_g, CMPS03* _boussole)
{ 
    moteur_g = _moteur_g;
    moteur_d = _moteur_d;
    boussole = _boussole;
    angle = 0;
    V_tout_droit = 1;
}

void PID_boussole::aller_tout_droit()
{
    moteur_g->speed(1);
    moteur_d->speed(0.76);
}

void PID_boussole::fPID_boussole()
{
    if(abs(Cconsigne)<=360)
    {
        angle = boussole->getBearing();
        Cact = angle;
    
        V_tourne = abs(angle-Cconsigne)/(360.0)+0.55;
        if(V_tourne>1) V_tourne=1;
    
        if(angle<(Cconsigne+ANGLE_TOLERANCE) && angle>(Cconsigne-ANGLE_TOLERANCE))
        {
            aller_tout_droit();
            flag_bon_sens = 1;
        }
        else
        {
            flag_bon_sens = 0;
            if((Cconsigne - Cact) <= 0)
            {
                moteur_g->speed(V_tourne);
                moteur_d->speed(0);
            }
            else
            {
                moteur_g->speed(0);
                moteur_d->speed(V_tourne+0.1);
            }
        }   
    }
    else flag_bon_sens = 1;
    
}

void PID_boussole::STOP_PID_boussole()
{
    tickPID_boussole.detach();
}

void PID_boussole::START_PID_boussole()
{
    tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
}

void PID_boussole::PID_boussole_consigne(int cons)
{
    Cconsigne = cons;
}