dalal dehmani
Projet véhicules autonomes
Dependencies: FileSystem_POPS TextLCD m3pi mbed
Fork of
m3PI_TP_POPS_II2015v0
by 
Samir Bouaziz
Diff: main3.cpp
- Revision:
- 4:a4ac92030374
- Parent:
- 0:398afdd73d9e
--- 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