File content as of revision 0:6004a7230f87:

#include "Moteur.h"

//t_asserv en seconde
Moteur::Moteur(PinName pin_pwm1, PinName pin_pwm2, PinName pin_feedback, PinName pin_capteur1, PinName pin_capteur2) : 
    m1(pin_pwm1),m2(pin_pwm2),
    feedbackCurrent(pin_feedback),
    capteur1(pin_capteur1),
    capteur2(pin_capteur2),
    tmr() 
{
    this->derniere_mesure = 0;
    //INIT DES PWM
    this->m1.period(MOT_PERIOD);
    this->m2.period(MOT_PERIOD);
    this->setPuissance(0);

    //INIT CAPTEUR
    this->capteur1.rise(this,&Moteur::interruptCapteurRise);
    this->capteur1.fall(this,&Moteur::interruptCapteurFall);
}

/*-1 : full marche arrière
 1 : full marche avant
 */
void Moteur::setPuissance(float p)
{
    if (p>0) {
        this->m1.write(0);
        this->m2.write(p);
    }else{
        this->m1.write(-p);
        this->m2.write(0);
    }
}

void Moteur::test1()
{
        //RAZ DES SORTIES
        this->m1.write(0.0f);
        this->m2.write(0.0f);
        
        //DEBUT SEQUENCE DE TEST
        xbee.printf("Debut Test Propulsion\n");
        this->m1.write(0.5);
        xbee.printf("M1 = 0.5\n");
        wait(10.0f);
        this->m1.write(0);
        this->m2.write(0.5);
        xbee.printf("M2 = 0.5\n");
        wait(10.0f);
        this->m2.write(0);
        xbee.printf("Fin Test Propulsion\n");
}

void Moteur::test2()
{
        //RAZ DES SORTIES
        this->m1.write(0.0f);
        this->m2.write(0.0f);
        
        //DEBUT SEQUENCE DE TEST
        xbee.printf("Debut Test Propulsion\n");
        this->m1.write(0.5);
        xbee.printf("M1 = 0.5\n");
        wait(10.0f);
        this->m1.write(0);
        this->m2.write(0.5);
        xbee.printf("M2 = 0.5\n");
        wait(10.0f);
        this->m2.write(0);
        xbee.printf("Fin Test Propulsion\n");
}

    
void Moteur::etalonnage() {
    xbee.printf("ETALONNAGE\n");
    for(int i=0;i<256;i++) { 
        this->m1.write((float)i/256.0f);
        xbee.printf("D1 : %d\n",i);
        wait(0.1f);
    }
}


void Moteur::asserv()
{
    //Application de la nouvelle commande
    this->setPuissance(regulateur.getCommande(this->erreur));
}

void Moteur::setKp(float k)
{
    this->regulateur.setKp(k);
}
void Moteur::setKi(float k)
{
    this->regulateur.setKp(k);
}
void Moteur::setKd(float k)
{
    this->regulateur.setKp(k);
}


void Moteur::interruptCapteurRise()
{
    this->mesure();
    this->majErreur();
    this->asserv();
}

void Moteur::interruptCapteurFall()
{
    this->mesure();
    this->majErreur();
    this->asserv();
}

void Moteur::mesure()
{
    this->derniere_mesure = 1/this->tmr.read();
    if(this->capteur2) {
        this->derniere_mesure = -this->derniere_mesure;
    }
    this->tmr.reset();
}

void Moteur::majErreur()
{
    float e_prec = this->erreur.p;
    
    this->erreur.p = this->derniere_mesure - this->vitesse_consigne;
    this->erreur.i += this->erreur.p;
    this->erreur.d = this->erreur.p - e_prec;
}

float Moteur::getMesure()
{
    return this->derniere_mesure;
}

void Moteur::setVitesse(float v)
{
    this->vitesse_consigne = v;
}