File content as of revision 4:a4ac92030374:

//Alaa et Dalal
#include "mbed.h"
#include "m3pi.h"
#include "MSCFileSystem.h"
#include "TextLCD.h"


m3pi m3pi;                  // Initialise the m3pi
TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD20x4); // rs, e, d4-d7
Serial xbee(p28,p27);
DigitalOut resetxbee(p26);
Serial pc(USBTX, USBRX);   // For debugging and pc
Timer t;
Ticker tick1;
int count=0;
unsigned char vecteur;
bool cmd;
int valeur;
int k=1;
float co;

void test();
BusOut myleds(LED1, LED2, LED3, LED4);

#define D_TERM 0.0
#define I_TERM 0.1
#define I_TERMO 0.1
#define P_TERM 0.9
#define MAX  0.3
#define MIN -0.2

#define seuil(x)  (x>600 ? 1 : 0)

float current_pos_of_line,
      previous_pos_of_line,
      derivate,
      proportional,
      power,
      integral,
      left,
      right,
      speed=0.2;
char  chain[15];
int   v;
char  delai600ms;
int   etat;
char etat1;
float correction = 0.2;
char  aff[10],aff1[10];
unsigned short tabsensor[5];
      //right= (right>MAX ? MAX :(right<MIN ? MIN : right));
      //left= (left>MAX ? MAX :(left<MIN ? MIN : left));
unsigned char statecapt;
bool mu;
int countt,countt1;
char  save[255]="             ";//"LRBRLRBW";
char  save1[15]="00000000000000";
int j=0;



//PID
char PIDf(char commande) 
{
    if(commande==1)
    {
        if(statecapt==0x1F)
        {
            m3pi.left_motor(0);
            m3pi.right_motor(0);
            m3pi.forward(speed);
            myleds=0xF;
            return 0;
        }
        else
        {
            // Get the position of the line
            current_pos_of_line = m3pi.line_position();
            proportional = current_pos_of_line;
            // Compute the derivate
            derivate = current_pos_of_line - previous_pos_of_line;
            // Compute the integral
            integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
            // Remember the last postion
            previous_pos_of_line = current_pos_of_line;
            // Compute the power
            power = (proportional*(P_TERM)) + (integral*(I_TERM)) +
            (derivate*(D_TERM));
            // Compute new speeds
            right = speed-(power*MAX);
            left = speed+(power*MAX);
            // Limits checks on motor control
            right= (right>MAX ? MAX :(right<MIN ? MIN : right));
            left= (left>MAX ? MAX :(left<MIN ? MIN : left));
            // Send command to motors
            m3pi.left_motor(left);
            m3pi.right_motor(right);
        }
    }
    return 1;
}
  
//Fonction superviseur
//////////////////////////////////////*******************************optimisation de chaine***************////////////////////////////////////
void optim(char save[], int countt)
{
  int i;
  int k;
  for(i=0;i<countt;i++)
  {

     if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='R'))
     {
                save[i]='W';
                for (k=1; k<(countt-i);k++)
                {
                    save[i+k]=save[i+k+2];
                }
                    i++;
     }
     if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='R'))
     {
                save[i]='L';
                for (k=1; k<(countt-i);k++)
                {
                    save[i+k]=save[i+k+2];
                }
                    i++;
                }
     if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='W'))
     {
                save[i]='B';
                for (k=1; k<(countt-i);k++)
                {
                save[i+k]=save[i+k+2];
                }
                i++;
     }
                
     if ((save[i]=='L')&(save[i+1]=='B')&(save[i+2]=='R'))
     {
                save[i]='B';
                for (k=1; k<(countt-i);k++)
                {
                save[i+k]=save[i+k+2];
                }
                i++;
     }
     if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='L'))
     {
                save[i]='B';
                for (k=1; k<(countt-i);k++)
                {
                save[i+k]=save[i+k+2];
                }
                i++;
     }
     if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='W'))
     {
                save[i]='L';
                for (k=1; k<(countt-i);k++)
                {
                save[i+k]=save[i+k+2];
                }
                i++;
     }
          
     else 
     {
                for (k=0; k<(countt-1);k++)
                {
                save[k]=save[k];
                }
     }
                    
}}



/////////////////////////////////////*****************************************************optimisation**************************/////////////////////////
void test1 ()
{
     int i;
     m3pi.calibrated_sensors(tabsensor);    
     for(i=0; i<5; i++) 
     {
        
       statecapt = (statecapt << 1) | seuil(tabsensor[i]);
        
     }
     vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+
                  (tabsensor[3]>600)*8+(tabsensor[4]>600)*16;
                  
     if(k==1)
     {             
        if ((tabsensor[0]<400)&(tabsensor[4]<400))
        {
           if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400))
           { 
                etat1 ='P';
           }
           else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat1=save[countt1];countt1++;}
        }
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<100)&(tabsensor[4]>600)) )
        {
                etat1=save[countt1];countt1++;
        }
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]<100)) )
        {
                etat1=save[countt1];countt1++;
        }                            
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix
        { 
            etat1=save[countt1] ;countt1++; 
        }
     }
switch (etat1)
{
         case 'B':
         m3pi.left_motor(speed);
         m3pi.right_motor(-speed);
         strcpy( aff,"back         ");
         wait(0.74);
         PIDf(1);
         wait(0.1);
         break;
           
         case 'E':
         m3pi.stop();
         m3pi.cls();
         m3pi.locate(0,1);
         m3pi.printf(save);;
         wait(1000);   
         break;
        
         case 'R' :
         strcpy( aff,"Right        "); 
         m3pi.left_motor(0);
         m3pi.right_motor(0);
         wait(0.01);
         m3pi.left_motor(1.2*(1.5/2)*speed);
         m3pi.right_motor(1.2*(1.5/2)*speed);
         wait(0.16);
         save[countt-1]='R';
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor(-(1.5/2)*speed);
         wait(0.5);
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor((1.5/2)*speed);
         wait(0.1);
         PIDf(1);
         wait(0.001);
         m3pi.calibrated_sensors(tabsensor);     
         break;
        
         case 'L' :
         m3pi.left_motor(0);
         m3pi.right_motor(0);
         wait(0.01);
         m3pi.right_motor(1.2*(1.5/2)*speed);
         m3pi.left_motor(1.2*(1.5/2)*speed);
         wait(0.16);
         save[countt-1]='R';
         m3pi.right_motor((1.5/2)*speed);
         m3pi.left_motor(-(1.5/2)*speed);
         wait(0.42);
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor((1.5/2)*speed);
         wait(0.1);
         PIDf(1);
         wait(0.001);
         m3pi.calibrated_sensors(tabsensor);      
         break;
   
         case 'W':
         m3pi.left_motor(0);
         m3pi.right_motor(0);
         wait(0.01);
         m3pi.left_motor(2*(1.5/2)*0.8*speed);
         m3pi.right_motor(2*(1.5/2)*0.8*speed);
         wait(0.14);
         strcpy( aff,"t_forward     ");
         PIDf(1);
         m3pi.left_motor(0);
         m3pi.right_motor(0);
         strcpy( aff,"w");
         m3pi.locate(0,1);
         m3pi.printf(aff);
         m3pi.stop();
         wait(0.01);
         PIDf(1);
         wait(0.1); 
         break;
            
         case 'P':
         PIDf(1);
         sprintf(aff,"%d",countt);
         m3pi.locate(0,1);
         m3pi.printf(aff);
         wait(0.001);
         break;
                                                
}}


///////////////////////////////////////////////////////***************Superviseur principal**************/////////////////////////////
void test ()
{
     int i;
     m3pi.calibrated_sensors(tabsensor);    
     for(i=0; i<5; i++) 
     {
        
       statecapt = (statecapt << 1) | seuil(tabsensor[i]);
        
     }
     vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+
                  (tabsensor[3]>600)*8+(tabsensor[4]>600)*16;
                  
     if(k==1)
     {             
        if ((tabsensor[0]<400)&(tabsensor[4]<400))
        {
            if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400))
                etat =2;
            else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat =0;countt++;}
        }
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<400)&(tabsensor[4]>600)) )
        {
                etat=1;countt++;
        }
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>800)&(tabsensor[4]<100)) )
        {
                etat=4;countt++;
        }
        else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix
        { 
                etat=3 ;countt++; 
        }
      }
 switch (etat)
 {
    case 0:
    m3pi.left_motor(1.2*(1.5/2)*speed);
    m3pi.right_motor(1.2*(1.5/2)*speed);
    wait(0.1);
    m3pi.left_motor(speed);
    m3pi.right_motor(-speed);
    strcpy( aff,"back         ");
    wait(0.74);
    PIDf(1);
    wait(0.1);
    save[countt-1]='B';
    break;
    case 1 :
    strcpy( aff,"Right        "); 
    m3pi.left_motor(0);
    m3pi.right_motor(0);
    wait(0.01);
    m3pi.left_motor(1.2*(1.5/2)*speed);
    m3pi.right_motor(1.2*(1.5/2)*speed);
    wait(0.16);
    save[countt-1]='R';
    m3pi.left_motor((1.5/2)*speed);
    m3pi.right_motor(-(1.5/2)*speed);
    wait(0.42);
    m3pi.left_motor((1.5/2)*speed);
    m3pi.right_motor((1.5/2)*speed);
    wait(0.1);
    PIDf(1);
    wait(0.001);
    m3pi.calibrated_sensors(tabsensor); 
    break;
    case 2:
    PIDf(1);
    sprintf(aff,"%d",countt);//strcpy( aff,"bye nombre");
    m3pi.locate(0,1);
    m3pi.printf(aff);
    m3pi.calibrated_sensors(tabsensor); 
    break;
    case 3:
    m3pi.left_motor(0);
    m3pi.right_motor(0);
    wait(0.01);
    m3pi.left_motor(1.2*(1.5/2)*speed);
    m3pi.right_motor(1.2*(1.5/2)*speed);
    wait(0.16);
    strcpy( aff,"t_forward     ");
    m3pi.calibrated_sensors(tabsensor);
    wait(0.02);
    if ((tabsensor[0]>600)&(tabsensor[4]>200)&(tabsensor[2]>600)&(tabsensor[3]>600)&(tabsensor[1]>600))//the end
    {
         save[countt-1]='E';
         m3pi.left_motor(2*0);
         m3pi.right_motor(2*0);
         cmd=false;
         m3pi.leds(255);
         wait(2);
         strcpy( aff,"Optim     ");
         m3pi.leds(255);
         m3pi.locate(0,1);
         m3pi.printf(aff);
         wait(0.5);
         m3pi.leds(255);
         wait(0.5);
         m3pi.leds(127);
         wait(0.5);
         m3pi.leds(63);
         wait(0.5);
         strcpy( aff,"2              ");
         m3pi.locate(0,1);
         m3pi.printf(aff);
         m3pi.leds(31);
         wait(0.5);
         m3pi.leds(15);  
         wait(0.5);
         m3pi.leds(7);
         strcpy( aff,"1           ");
         m3pi.locate(0,1);
         m3pi.printf(aff);
         m3pi.leds(3);
         wait(0.5);
         m3pi.leds(1);
         wait(0.5);
         strcpy( aff,"0           ");
         m3pi.locate(0,1);
         m3pi.printf(aff);
         wait(1); 
         m3pi.leds(0);
         strcpy( aff,"GOOOOOOOOOO");
         m3pi.locate(0,1);
         m3pi.printf(aff);
         wait(1); 
    }
    else        
    { 
         save[countt-1]='R';
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor(-(1.5/2)*speed);
         wait(0.53);
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor((1.5/2)*speed);
         wait(0.1);
         PIDf(1);
         wait(0.001);
         strcpy( aff,"turned     ");
    }
       
    break;
    case 4:
    m3pi.left_motor(0);
    m3pi.right_motor(0);
    wait(0.01);
    m3pi.left_motor(2*(1.5/2)*0.8*speed);
    m3pi.right_motor(2*(1.5/2)*0.8*speed);
    wait(0.14);
    strcpy( aff,"t_forward     ");
    PIDf(1);
    m3pi.left_motor(0);
    m3pi.right_motor(0);
    m3pi.calibrated_sensors(tabsensor);
    save[countt-1]='W';    
    if (((tabsensor[0]<400)&(tabsensor[4]<400))&(tabsensor[2]>900))//forward     
    {   
         PIDf(1);
         wait(0.001);
    }            
    if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100))//turn left
    {  
         save[countt-1]='L';
         m3pi.left_motor(-(1.5/2)*speed);
         m3pi.right_motor((1.5/2)*speed);
         wait(0.48);
         m3pi.left_motor((1.5/2)*speed);
         m3pi.right_motor((1.5/2)*speed);
         wait(0.21);
    }
    break;
}}
       ////////////////////////*********************************************/////////////////////////////////////////////////
       
volatile char flag10ms;

void inter1()
{
    flag10ms=1;
    
}

int main() 
{
    k=1;
    countt=0;
    countt1=0;
    cmd=1;
    resetxbee =0;
    wait(0.01);
    resetxbee =1;
    statecapt=0;
    m3pi.sensor_auto_calibrate();
    wait(1.);
    tick1.attach(&inter1,0.01);
    mu=1;
    while(1)
    {
       if (1)
       {
            if(flag10ms==1)
            {
                flag10ms=0;
                t.reset();
                t.start();
                m3pi.calibrated_sensors(tabsensor);
                if(cmd)
                {
                    test();
                }
                else 
                {
                    optim(save,countt);optim(save,countt);optim(save,countt);optim(save,countt);   
                    test1();
                }
                t.stop();
                v=t.read_us();
                delai600ms++;
            }
      }
}  

for (k=0 ;k<countt; k++ )
{
    if(k<8)
    {
     aff[k]=save[k];
    }
    else
    {
     aff1[k]=save1[k];
    }

}
    m3pi.locate(0,1);
    m3pi.printf(aff);
  
}