Revision 4:a4ac92030374, committed 2018-03-12

Comitter:

Dalaldeh

Date:

Mon Mar 12 00:10:49 2018 +0000

Parent:

3:b4b0c5219d2a

Commit message:

Projet v?hicules autonomes

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Mon Mar 12 00:10:49 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/simon/code/TextLCD/#308d188a2d3a

--- a/main3.cpp	Mon Nov 23 23:24:35 2015 +0000
+++ b/main3.cpp	Mon Mar 12 00:10:49 2018 +0000
@@ -1,31 +1,546 @@
-
+//Alaa et Dalal
 #include "mbed.h"
 #include "m3pi.h"
 #include "MSCFileSystem.h"
+#include "TextLCD.h"
 
-m3pi m3pi;                                  // Initialise the m3pi
 
+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 messages, uses commented out to prevent hanging
-MSCFileSystem fs("fs"); 
+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);
 
-int main() {
+#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;
-    
-    FILE *p= fopen("/fs/tt.txt","a+");
+    statecapt=0;
     m3pi.sensor_auto_calibrate();
     wait(1.);
-    
-    fprintf(p,"ecrire dans la cle USB\r\n");
-    fclose(p);
-    
-    while(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);
+  
+}
+ 
\ No newline at end of file