Projet véhicules autonomes

Dependencies:   FileSystem_POPS TextLCD m3pi mbed

Fork of m3PI_TP_POPS_II2015v0 by Samir Bouaziz

Files at this revision

API Documentation at this revision

Comitter:
Dalaldeh
Date:
Mon Mar 12 00:10:49 2018 +0000
Parent:
3:b4b0c5219d2a
Commit message:
Projet v?hicules autonomes

Changed in this revision

TextLCD.lib Show annotated file Show diff for this revision Revisions of this file
main3.cpp Show annotated file Show diff for this revision Revisions of this file
--- /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