File content as of revision 0:e2ce7aa2a4a4:

#include "mbed.h"
#include "lib.h"
Timer temps;
DigitalOut sensG(D7);
DigitalOut sensD(D9);
PwmOut motG(D6);
PwmOut motD(D8);
DigitalIn BG(D14);
InterruptIn AG(D13);
InterruptIn AD(D12);
DigitalIn BD(D11);
int cptG=0,cptD=0;
int cPosG=0,cPosD=0;
int erD,somErD=0;
int erG,somErG=0;
int cmdD=0,cmdG=0;
float cPos;
Ticker tic;
Ticker tac;
void asserPos()
{
    erD=cPosD-cptD;
    somErD=somErD+erD;
    if(somErD>100000)somErD=100000;
    if(somErD<-100000)somErD=-100000;
    cmdD=K*erD+K*Te*somErD/Ti;

    erG=cPosG-cptG;
    somErG=somErG+erG;
    if(somErG>100000)somErG=100000;
    if(somErG<-100000)somErG=-100000;
    cmdG=K*erG+K*Te*somErG/Ti;

    mot(cmdG,cmdD);
}
void distance(int d)//distance passee en mm
{
    int signe;
    if(d>0) signe=0;
    else {
        signe=1;
        d=-1*d;
    }
    cptG=0;
    cptD=0;
    float dlim=1000*V*V/A; //valeur limite entre le profil en trapeze et celui en triangle

    if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
    if(d>dlim) { // profil en trapeze

        float t0=V/A;
        float t1=d/(V*1000.0);
        if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
        temps.reset();
        temps.start();
        cPosG=0;
        cPosD=0;
        tic.attach(&asserPos,Te); // on lance l'asservissement de position
        float t=0;
        while(t<t0) {
            t=temps.read();
            cPos=A*t*t/2.0;
            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
            else cPosD=-1*cPos*1000.0/mmPULSE;
        }
        while(t<t1) {
            t=temps.read();
            cPos=(A*t0*t0/2.0)+(V*(t-t0));
            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
            else cPosD=-1*cPos*1000.0/mmPULSE;
        }
        while(t<(t1+t0)) {
            t=temps.read();
            cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
            else cPosD=-1*cPos*1000.0/mmPULSE;
        }
        while(t<(t1+t0+DELAY)) {
            t=temps.read();
        }
        if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
        tic.detach();
        mot(0,0);
    } else {            //profil en triangle
        float tA=sqrt(d/(1000.0*A));
        float t2A=2*tA;
        if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
        temps.reset();
        temps.start();
        cPosG=0;
        cPosD=0;
        tac.attach(&asserPos,Te);
        float t=0;
        while(t<tA) {
            t=temps.read();
            cPos=A*t*t/2;
            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
            else cPosD=-1*cPos*1000.0/mmPULSE;
        }
        while(t<t2A) {
            t=temps.read();
            cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
            else cPosD=-1*cPos*1000.0/mmPULSE;
        }
        while(t<(t2A+DELAY)) {
            t=temps.read();
        }
        if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
        tac.detach();
        mot(0,0);
    }
    temps.stop();
    temps.reset();

}
void rotation(int deg)
{

    int d=deg*ENTREAXE/360.0;
    int signe;
    if(d>0) signe=0;
    else {
        signe=1;
        d=-1*d;
    }
    cptG=0;
    cptD=0;
    float dlim=1000*V*V/A;

    if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
    if(d>dlim) {

        float t0=V/A;
        float t1=d/(V*1000.0);
        if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
        temps.reset();
        temps.start();
        cPosG=0;
        cPosD=0;
        tic.attach(&asserPos,Te);
        float t=0;
        while(t<t0) {
            t=temps.read();
            cPos=A*t*t/2.0;
            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
            else cPosD=cPos*1000.0/mmPULSE;
        }
        while(t<t1) {
            t=temps.read();
            cPos=(A*t0*t0/2.0)+(V*(t-t0));
            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
            else cPosD=cPos*1000.0/mmPULSE;
        }
        while(t<(t1+t0)) {
            t=temps.read();
            cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
            else cPosD=cPos*1000.0/mmPULSE;
        }
        while(t<(t1+t0+DELAY)) {
            t=temps.read();
        }
        if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
        tic.detach();
        mot(0,0);
    } else {
        float tA=sqrt(d/(1000.0*A));
        float t2A=2*tA;
        if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
        temps.reset();
        temps.start();
        cPosG=0;
        cPosD=0;
        tac.attach(&asserPos,Te);
        float t=0;
        while(t<tA) {
            t=temps.read();
            cPos=A*t*t/2;
            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
            else cPosD=cPos*1000.0/mmPULSE;
        }
        while(t<t2A) {
            t=temps.read();
            cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
            else cPosG=-1*cPos*1000.0/mmPULSE;
            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
            else cPosD=cPos*1000.0/mmPULSE;
        }
        while(t<(t2A+DELAY)) {
            t=temps.read();
        }
        if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
        tac.detach();
        mot(0,0);
    }
    temps.stop();
    temps.reset();


}
void init()
{
    motG.period_us(100);
    motD.period_us(100);
    mot(0,0);
    AG.rise(&countGr);
    AG.fall(&countGf);
    AD.rise(&countDr);
    AD.fall(&countDf);
    //tic.attach(&asserPos,Te);

}
void countGr()
{
    if(BG==1)
        cptG++;
    else
        cptG--;
}
void countDr()
{
    if(BD==0)
        cptD++;
    else
        cptD--;
}
void countGf()
{
    if(BG==0)
        cptG++;
    else
        cptG--;
}
void countDf()
{
    if(BD==1)
        cptD++;
    else
        cptD--;
}


void aff()
{
    float vD=30*cptD/(22.0*5.0*Te);
    float vG=30*cptG/(22.0*5.0*Te);
    int valD=100*motD.read();
    int valG=100*motG.read();
    //printf("cptG=%d -- motG=%.3f cm/s -- cptD=%d -- motD=%.3f cm/s -- valD=%d\n\r",cptG,vG,cptD,vD,valD);


}
int borne(int a)
{
    if(a>100) a=100;
    if(a<-100) a=-100;
    return a;
}
void mot(int g,int d)
{
    g=borne(g);
    d=borne(d);
    if(g>0) {
        sensG.write(1);
        g=100-g;
        motG.pulsewidth_us(g);
    } else {
        sensG.write(0);
        g=100+g;
        motG.pulsewidth_us(g);
    }
    if(d>0) {
        sensD.write(1);
        d=100-d;
        motD.pulsewidth_us(d);
    } else {
        sensD.write(0);
        d=100+d;
        motD.pulsewidth_us(d);
    }
}