File content as of revision 1:ef658023014b:

#include "mbed.h"
#include "m3pi.h"
//#include "MSCFileSystem.h"

//Serial usb(USBTX,USBRX);
Serial usb(p28,p27);

DigitalOut l1(LED1);
DigitalOut l2(LED2);
DigitalOut l3(LED3);
DigitalOut l4(LED4);

m3pi m3pi;

// Minimum and maximum motor speeds
#define MAX 0.5
#define MIN 0

// PID terms
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

#define THRESHOLD 700

void PID(const float &cur, const float& prev, float& I, float& right, float& left)
{
    float K;
    float V = MAX;
    float P,D=0;

    // On calcul le proportionnel      
    P = cur;
        
    // Calcul de la derivee
    D = cur - prev;
        
    // Calcul de l'intégral
    I += P;
    
    // On calcul le terme de correction
    K = (P*(P_TERM)) + (I*(I_TERM)) + (D*(D_TERM)) ;
        
    // On calcule les nouvelles vitesses   
    float tmpr = V+K;
    float tmpl  = V-K;
        
    // On verifie les bornes
    if (tmpr < MIN)         tmpr = MIN;
    else if (tmpr > MAX)    tmpr = MAX;
    
    if (tmpl < MIN)         tmpl = MIN;
    else if (tmpl > MAX)    tmpl = MAX;  
    
    right=tmpr; left=tmpl;  
}

void seuillage(int *cap, char *caps)
{
    for(int i=0; i<5; i++){
        if(cap[i] >= THRESHOLD)
            caps[i] = 1;
        else 
            caps[i] = 0;
    }
}

void getState(char* caps, char *state, char prev_state)
{
    if(prev_state==0)   //Initialisation
        *state=1;
    else if(prev_state==1)
    {
        if(caps[4]==1)
            *state=2;   //virage a droite
        else if(caps[0]==1)
            *state=3;   //virage a gauche
        else
            *state=1;   //ligne droite
    }
    else if(prev_state==2)              //Si on était sur un virage a droite
    {
        if(caps[0]==0 && caps[4]==0)    //et qu'on revient sur une ligne droite
           *state=1; 
    }
    else if(prev_state==3)              //Si on était sur un virage a droite
    {
        if(caps[0]==0 && caps[4]==0)
            *state=1;
    }
}

//Fonction de correction
float getCorr(char state)
{
    if(state==1)        
        return 2.0;
    else
    {
    if(state==2)
        return 0.7;
    else if(state==3)
        return -0.7;
    }
}

int main() {
    wait(1.0);
    m3pi.sensor_auto_calibrate();

    float right;
    float left;
    float cur = 0.0;
    float prev = 0.0;
    float I=0;
    
    int cap[5]={1,1,1,1,1};
    char caps[5]={0,0,0,0,0};
    
    char state, prev_state=0;
    
    float corr = 0.0;
    
    while (1) {
        //lecture du sensor
        m3pi.readsensor(cap);
        seuillage(cap,caps);
        
        //On recupere l'etat
        prev_state=state;
        getState(caps,&state,prev_state);
        corr=getCorr(state);
        
        if(corr==2.0)       //Signifie qu'on est sur une ligne droite
            cur=m3pi.line_position();
        else                //Signifie qu'on est dans un etat particulier
            cur=corr;
        
        /*
        //Condition d'etat
        cur = m3pi.line_position();     // On recupere la position de la ligne  
         */
         
        PID(cur,prev,I,right,left);     //On calcule le PID
        prev = cur;                     // Memorisation de l'etat precedent
          
        //Envoi de la commande
        m3pi.left_motor(left);
        m3pi.right_motor(right);
    }
}


/*
float speed = 0.1;
int seuil = 700;

int main() { 

if(usb.writable())
    usb.printf("Hello World");   

int capteurs[5]={1,1,1,1,1};
int cap_seuil[5]={0,0,0,0,0};
float line_detec=0;
char buff[8];
char c,cprev;
while(1) 
{
        //Boucle switch de reception de données via serial
        if(usb.readable()){
            cprev=c;
            c = usb.getc();
            switch(c){ 
            case 'z':
                pololu.forward(speed);
                l1 = 1;
                l2 = 0;
                l3 = 0;
                l4 = 0;
                break;
            case 's':    
                pololu.backward(speed);
                l1 = 0;
                l2 = 1;
                l3 = 0;
                l4 = 0;
                break;
            case 'q':    
                pololu.left(speed);
                l1 = 1;
                l2 = 0;
                l3 = 0;
                l4 = 0;
                break;
            case 'd':    
                pololu.right(speed);
                l1 = 1;
                l2 = 0;
                l3 = 0;
                l4 = 0;
                break;
            case ' ':    
                pololu.stop();
                l1 = 1;
                l2 = 1;
                l3 = 1;
                l4 = 1;
                break;
                
            case 'c':
                pololu.readsensor(capteurs);
                for(int i=0; i<5; i++)
                {
                    if(capteurs[i] >= seuil)
                    {
                        cap_seuil[i] = 1;
                    }
                    else 
                    {
                        cap_seuil[i] = 0;
                    }
                }

                 if(usb.writable()) 
                 {
                        usb.printf("Valeur capteur :  \r\n");
                        usb.printf("Valeurs brutes: %d, %d, %d, %d, %d  \r\n", (int)capteurs[0],(int)capteurs[1],(int)capteurs[2],(int)capteurs[3],(int)capteurs[4]);
                        usb.printf("Valeurs seuillees: %d, %d, %d, %d, %d  \r\n \r\n ",cap_seuil[0],cap_seuil[1],cap_seuil[2],cap_seuil[3],cap_seuil[4]);
                }
                break;
                
            case '0':
                pololu.sensor_auto_calibrate();
                break;
                
            case 'b':
                sprintf(buff, "Bat:%3.f",pololu.battery());
                pololu.cls();
                pololu.locate(0,0);
                pololu.print(buff,8);
            
            case 'l':
                line_detec = pololu.line_position();
                if(usb.writable())
                        usb.printf("Position ligne : %f\r\n",line_detec);
                            
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
            default : break; 
            }
        }
    }
}
*/