dalal dehmani / Mbed 2 deprecated ourproject

Projet véhicules autonomes

Dependencies:   FileSystem_POPS TextLCD m3pi mbed

Fork of m3PI_TP_POPS_II2015v0 by Samir Bouaziz

main3.cpp@4:a4ac92030374, 2018-03-12 (annotated)

Committer:
Dalaldeh
Date:
Mon Mar 12 00:10:49 2018 +0000
Revision:
4:a4ac92030374
Parent:
0:398afdd73d9e 
Projet v?hicules autonomes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Dalaldeh 4:a4ac92030374 1 //Alaa et Dalal
bouaziz 0:398afdd73d9e 2 #include "mbed.h"
bouaziz 0:398afdd73d9e 3 #include "m3pi.h"
bouaziz 0:398afdd73d9e 4 #include "MSCFileSystem.h"
Dalaldeh 4:a4ac92030374 5 #include "TextLCD.h"
bouaziz 0:398afdd73d9e 6
bouaziz 0:398afdd73d9e 7
Dalaldeh 4:a4ac92030374 8 m3pi m3pi; // Initialise the m3pi
Dalaldeh 4:a4ac92030374 9 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD20x4); // rs, e, d4-d7
bouaziz 0:398afdd73d9e 10 Serial xbee(p28,p27);
bouaziz 0:398afdd73d9e 11 DigitalOut resetxbee(p26);
Dalaldeh 4:a4ac92030374 12 Serial pc(USBTX, USBRX); // For debugging and pc
Dalaldeh 4:a4ac92030374 13 Timer t;
Dalaldeh 4:a4ac92030374 14 Ticker tick1;
Dalaldeh 4:a4ac92030374 15 int count=0;
Dalaldeh 4:a4ac92030374 16 unsigned char vecteur;
Dalaldeh 4:a4ac92030374 17 bool cmd;
Dalaldeh 4:a4ac92030374 18 int valeur;
Dalaldeh 4:a4ac92030374 19 int k=1;
Dalaldeh 4:a4ac92030374 20 float co;
bouaziz 0:398afdd73d9e 21
Dalaldeh 4:a4ac92030374 22 void test();
bouaziz 0:398afdd73d9e 23 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 24
Dalaldeh 4:a4ac92030374 25 #define D_TERM 0.0
Dalaldeh 4:a4ac92030374 26 #define I_TERM 0.1
Dalaldeh 4:a4ac92030374 27 #define I_TERMO 0.1
Dalaldeh 4:a4ac92030374 28 #define P_TERM 0.9
Dalaldeh 4:a4ac92030374 29 #define MAX 0.3
Dalaldeh 4:a4ac92030374 30 #define MIN -0.2
Dalaldeh 4:a4ac92030374 31
Dalaldeh 4:a4ac92030374 32 #define seuil(x) (x>600 ? 1 : 0)
Dalaldeh 4:a4ac92030374 33
Dalaldeh 4:a4ac92030374 34 float current_pos_of_line,
Dalaldeh 4:a4ac92030374 35 previous_pos_of_line,
Dalaldeh 4:a4ac92030374 36 derivate,
Dalaldeh 4:a4ac92030374 37 proportional,
Dalaldeh 4:a4ac92030374 38 power,
Dalaldeh 4:a4ac92030374 39 integral,
Dalaldeh 4:a4ac92030374 40 left,
Dalaldeh 4:a4ac92030374 41 right,
Dalaldeh 4:a4ac92030374 42 speed=0.2;
Dalaldeh 4:a4ac92030374 43 char chain[15];
Dalaldeh 4:a4ac92030374 44 int v;
Dalaldeh 4:a4ac92030374 45 char delai600ms;
Dalaldeh 4:a4ac92030374 46 int etat;
Dalaldeh 4:a4ac92030374 47 char etat1;
Dalaldeh 4:a4ac92030374 48 float correction = 0.2;
Dalaldeh 4:a4ac92030374 49 char aff[10],aff1[10];
Dalaldeh 4:a4ac92030374 50 unsigned short tabsensor[5];
Dalaldeh 4:a4ac92030374 51 //right= (right>MAX ? MAX :(right<MIN ? MIN : right));
Dalaldeh 4:a4ac92030374 52 //left= (left>MAX ? MAX :(left<MIN ? MIN : left));
Dalaldeh 4:a4ac92030374 53 unsigned char statecapt;
Dalaldeh 4:a4ac92030374 54 bool mu;
Dalaldeh 4:a4ac92030374 55 int countt,countt1;
Dalaldeh 4:a4ac92030374 56 char save[255]=" ";//"LRBRLRBW";
Dalaldeh 4:a4ac92030374 57 char save1[15]="00000000000000";
Dalaldeh 4:a4ac92030374 58 int j=0;
Dalaldeh 4:a4ac92030374 59
Dalaldeh 4:a4ac92030374 60
Dalaldeh 4:a4ac92030374 61
Dalaldeh 4:a4ac92030374 62 //PID
Dalaldeh 4:a4ac92030374 63 char PIDf(char commande)
Dalaldeh 4:a4ac92030374 64 {
Dalaldeh 4:a4ac92030374 65 if(commande==1)
Dalaldeh 4:a4ac92030374 66 {
Dalaldeh 4:a4ac92030374 67 if(statecapt==0x1F)
Dalaldeh 4:a4ac92030374 68 {
Dalaldeh 4:a4ac92030374 69 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 70 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 71 m3pi.forward(speed);
Dalaldeh 4:a4ac92030374 72 myleds=0xF;
Dalaldeh 4:a4ac92030374 73 return 0;
Dalaldeh 4:a4ac92030374 74 }
Dalaldeh 4:a4ac92030374 75 else
Dalaldeh 4:a4ac92030374 76 {
Dalaldeh 4:a4ac92030374 77 // Get the position of the line
Dalaldeh 4:a4ac92030374 78 current_pos_of_line = m3pi.line_position();
Dalaldeh 4:a4ac92030374 79 proportional = current_pos_of_line;
Dalaldeh 4:a4ac92030374 80 // Compute the derivate
Dalaldeh 4:a4ac92030374 81 derivate = current_pos_of_line - previous_pos_of_line;
Dalaldeh 4:a4ac92030374 82 // Compute the integral
Dalaldeh 4:a4ac92030374 83 integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
Dalaldeh 4:a4ac92030374 84 // Remember the last postion
Dalaldeh 4:a4ac92030374 85 previous_pos_of_line = current_pos_of_line;
Dalaldeh 4:a4ac92030374 86 // Compute the power
Dalaldeh 4:a4ac92030374 87 power = (proportional*(P_TERM)) + (integral*(I_TERM)) +
Dalaldeh 4:a4ac92030374 88 (derivate*(D_TERM));
Dalaldeh 4:a4ac92030374 89 // Compute new speeds
Dalaldeh 4:a4ac92030374 90 right = speed-(power*MAX);
Dalaldeh 4:a4ac92030374 91 left = speed+(power*MAX);
Dalaldeh 4:a4ac92030374 92 // Limits checks on motor control
Dalaldeh 4:a4ac92030374 93 right= (right>MAX ? MAX :(right<MIN ? MIN : right));
Dalaldeh 4:a4ac92030374 94 left= (left>MAX ? MAX :(left<MIN ? MIN : left));
Dalaldeh 4:a4ac92030374 95 // Send command to motors
Dalaldeh 4:a4ac92030374 96 m3pi.left_motor(left);
Dalaldeh 4:a4ac92030374 97 m3pi.right_motor(right);
Dalaldeh 4:a4ac92030374 98 }
Dalaldeh 4:a4ac92030374 99 }
Dalaldeh 4:a4ac92030374 100 return 1;
Dalaldeh 4:a4ac92030374 101 }
Dalaldeh 4:a4ac92030374 102
Dalaldeh 4:a4ac92030374 103 //Fonction superviseur
Dalaldeh 4:a4ac92030374 104 //////////////////////////////////////*******************************optimisation de chaine***************////////////////////////////////////
Dalaldeh 4:a4ac92030374 105 void optim(char save[], int countt)
Dalaldeh 4:a4ac92030374 106 {
Dalaldeh 4:a4ac92030374 107 int i;
Dalaldeh 4:a4ac92030374 108 int k;
Dalaldeh 4:a4ac92030374 109 for(i=0;i<countt;i++)
Dalaldeh 4:a4ac92030374 110 {
Dalaldeh 4:a4ac92030374 111
Dalaldeh 4:a4ac92030374 112 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='R'))
Dalaldeh 4:a4ac92030374 113 {
Dalaldeh 4:a4ac92030374 114 save[i]='W';
Dalaldeh 4:a4ac92030374 115 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 116 {
Dalaldeh 4:a4ac92030374 117 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 118 }
Dalaldeh 4:a4ac92030374 119 i++;
Dalaldeh 4:a4ac92030374 120 }
Dalaldeh 4:a4ac92030374 121 if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='R'))
Dalaldeh 4:a4ac92030374 122 {
Dalaldeh 4:a4ac92030374 123 save[i]='L';
Dalaldeh 4:a4ac92030374 124 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 125 {
Dalaldeh 4:a4ac92030374 126 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 127 }
Dalaldeh 4:a4ac92030374 128 i++;
Dalaldeh 4:a4ac92030374 129 }
Dalaldeh 4:a4ac92030374 130 if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='W'))
Dalaldeh 4:a4ac92030374 131 {
Dalaldeh 4:a4ac92030374 132 save[i]='B';
Dalaldeh 4:a4ac92030374 133 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 134 {
Dalaldeh 4:a4ac92030374 135 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 136 }
Dalaldeh 4:a4ac92030374 137 i++;
Dalaldeh 4:a4ac92030374 138 }
Dalaldeh 4:a4ac92030374 139
Dalaldeh 4:a4ac92030374 140 if ((save[i]=='L')&(save[i+1]=='B')&(save[i+2]=='R'))
Dalaldeh 4:a4ac92030374 141 {
Dalaldeh 4:a4ac92030374 142 save[i]='B';
Dalaldeh 4:a4ac92030374 143 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 144 {
Dalaldeh 4:a4ac92030374 145 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 146 }
Dalaldeh 4:a4ac92030374 147 i++;
Dalaldeh 4:a4ac92030374 148 }
Dalaldeh 4:a4ac92030374 149 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='L'))
Dalaldeh 4:a4ac92030374 150 {
Dalaldeh 4:a4ac92030374 151 save[i]='B';
Dalaldeh 4:a4ac92030374 152 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 153 {
Dalaldeh 4:a4ac92030374 154 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 155 }
Dalaldeh 4:a4ac92030374 156 i++;
Dalaldeh 4:a4ac92030374 157 }
Dalaldeh 4:a4ac92030374 158 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='W'))
Dalaldeh 4:a4ac92030374 159 {
Dalaldeh 4:a4ac92030374 160 save[i]='L';
Dalaldeh 4:a4ac92030374 161 for (k=1; k<(countt-i);k++)
Dalaldeh 4:a4ac92030374 162 {
Dalaldeh 4:a4ac92030374 163 save[i+k]=save[i+k+2];
Dalaldeh 4:a4ac92030374 164 }
Dalaldeh 4:a4ac92030374 165 i++;
Dalaldeh 4:a4ac92030374 166 }
Dalaldeh 4:a4ac92030374 167
Dalaldeh 4:a4ac92030374 168 else
Dalaldeh 4:a4ac92030374 169 {
Dalaldeh 4:a4ac92030374 170 for (k=0; k<(countt-1);k++)
Dalaldeh 4:a4ac92030374 171 {
Dalaldeh 4:a4ac92030374 172 save[k]=save[k];
Dalaldeh 4:a4ac92030374 173 }
Dalaldeh 4:a4ac92030374 174 }
Dalaldeh 4:a4ac92030374 175
Dalaldeh 4:a4ac92030374 176 }}
Dalaldeh 4:a4ac92030374 177
Dalaldeh 4:a4ac92030374 178
Dalaldeh 4:a4ac92030374 179
Dalaldeh 4:a4ac92030374 180 /////////////////////////////////////*****************************************************optimisation**************************/////////////////////////
Dalaldeh 4:a4ac92030374 181 void test1 ()
Dalaldeh 4:a4ac92030374 182 {
Dalaldeh 4:a4ac92030374 183 int i;
Dalaldeh 4:a4ac92030374 184 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 185 for(i=0; i<5; i++)
Dalaldeh 4:a4ac92030374 186 {
Dalaldeh 4:a4ac92030374 187
Dalaldeh 4:a4ac92030374 188 statecapt = (statecapt << 1) | seuil(tabsensor[i]);
Dalaldeh 4:a4ac92030374 189
Dalaldeh 4:a4ac92030374 190 }
Dalaldeh 4:a4ac92030374 191 vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+
Dalaldeh 4:a4ac92030374 192 (tabsensor[3]>600)*8+(tabsensor[4]>600)*16;
Dalaldeh 4:a4ac92030374 193
Dalaldeh 4:a4ac92030374 194 if(k==1)
Dalaldeh 4:a4ac92030374 195 {
Dalaldeh 4:a4ac92030374 196 if ((tabsensor[0]<400)&(tabsensor[4]<400))
Dalaldeh 4:a4ac92030374 197 {
Dalaldeh 4:a4ac92030374 198 if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400))
Dalaldeh 4:a4ac92030374 199 {
Dalaldeh 4:a4ac92030374 200 etat1 ='P';
Dalaldeh 4:a4ac92030374 201 }
Dalaldeh 4:a4ac92030374 202 else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat1=save[countt1];countt1++;}
Dalaldeh 4:a4ac92030374 203 }
Dalaldeh 4:a4ac92030374 204 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<100)&(tabsensor[4]>600)) )
Dalaldeh 4:a4ac92030374 205 {
Dalaldeh 4:a4ac92030374 206 etat1=save[countt1];countt1++;
Dalaldeh 4:a4ac92030374 207 }
Dalaldeh 4:a4ac92030374 208 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]<100)) )
Dalaldeh 4:a4ac92030374 209 {
Dalaldeh 4:a4ac92030374 210 etat1=save[countt1];countt1++;
Dalaldeh 4:a4ac92030374 211 }
Dalaldeh 4:a4ac92030374 212 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix
Dalaldeh 4:a4ac92030374 213 {
Dalaldeh 4:a4ac92030374 214 etat1=save[countt1] ;countt1++;
Dalaldeh 4:a4ac92030374 215 }
Dalaldeh 4:a4ac92030374 216 }
Dalaldeh 4:a4ac92030374 217 switch (etat1)
Dalaldeh 4:a4ac92030374 218 {
Dalaldeh 4:a4ac92030374 219 case 'B':
Dalaldeh 4:a4ac92030374 220 m3pi.left_motor(speed);
Dalaldeh 4:a4ac92030374 221 m3pi.right_motor(-speed);
Dalaldeh 4:a4ac92030374 222 strcpy( aff,"back ");
Dalaldeh 4:a4ac92030374 223 wait(0.74);
Dalaldeh 4:a4ac92030374 224 PIDf(1);
Dalaldeh 4:a4ac92030374 225 wait(0.1);
Dalaldeh 4:a4ac92030374 226 break;
Dalaldeh 4:a4ac92030374 227
Dalaldeh 4:a4ac92030374 228 case 'E':
Dalaldeh 4:a4ac92030374 229 m3pi.stop();
Dalaldeh 4:a4ac92030374 230 m3pi.cls();
Dalaldeh 4:a4ac92030374 231 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 232 m3pi.printf(save);;
Dalaldeh 4:a4ac92030374 233 wait(1000);
Dalaldeh 4:a4ac92030374 234 break;
Dalaldeh 4:a4ac92030374 235
Dalaldeh 4:a4ac92030374 236 case 'R' :
Dalaldeh 4:a4ac92030374 237 strcpy( aff,"Right ");
Dalaldeh 4:a4ac92030374 238 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 239 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 240 wait(0.01);
Dalaldeh 4:a4ac92030374 241 m3pi.left_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 242 m3pi.right_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 243 wait(0.16);
Dalaldeh 4:a4ac92030374 244 save[countt-1]='R';
Dalaldeh 4:a4ac92030374 245 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 246 m3pi.right_motor(-(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 247 wait(0.5);
Dalaldeh 4:a4ac92030374 248 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 249 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 250 wait(0.1);
Dalaldeh 4:a4ac92030374 251 PIDf(1);
Dalaldeh 4:a4ac92030374 252 wait(0.001);
Dalaldeh 4:a4ac92030374 253 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 254 break;
Dalaldeh 4:a4ac92030374 255
Dalaldeh 4:a4ac92030374 256 case 'L' :
Dalaldeh 4:a4ac92030374 257 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 258 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 259 wait(0.01);
Dalaldeh 4:a4ac92030374 260 m3pi.right_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 261 m3pi.left_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 262 wait(0.16);
Dalaldeh 4:a4ac92030374 263 save[countt-1]='R';
Dalaldeh 4:a4ac92030374 264 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 265 m3pi.left_motor(-(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 266 wait(0.42);
Dalaldeh 4:a4ac92030374 267 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 268 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 269 wait(0.1);
Dalaldeh 4:a4ac92030374 270 PIDf(1);
Dalaldeh 4:a4ac92030374 271 wait(0.001);
Dalaldeh 4:a4ac92030374 272 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 273 break;
Dalaldeh 4:a4ac92030374 274
Dalaldeh 4:a4ac92030374 275 case 'W':
Dalaldeh 4:a4ac92030374 276 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 277 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 278 wait(0.01);
Dalaldeh 4:a4ac92030374 279 m3pi.left_motor(2*(1.5/2)*0.8*speed);
Dalaldeh 4:a4ac92030374 280 m3pi.right_motor(2*(1.5/2)*0.8*speed);
Dalaldeh 4:a4ac92030374 281 wait(0.14);
Dalaldeh 4:a4ac92030374 282 strcpy( aff,"t_forward ");
Dalaldeh 4:a4ac92030374 283 PIDf(1);
Dalaldeh 4:a4ac92030374 284 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 285 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 286 strcpy( aff,"w");
Dalaldeh 4:a4ac92030374 287 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 288 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 289 m3pi.stop();
Dalaldeh 4:a4ac92030374 290 wait(0.01);
Dalaldeh 4:a4ac92030374 291 PIDf(1);
Dalaldeh 4:a4ac92030374 292 wait(0.1);
Dalaldeh 4:a4ac92030374 293 break;
Dalaldeh 4:a4ac92030374 294
Dalaldeh 4:a4ac92030374 295 case 'P':
Dalaldeh 4:a4ac92030374 296 PIDf(1);
Dalaldeh 4:a4ac92030374 297 sprintf(aff,"%d",countt);
Dalaldeh 4:a4ac92030374 298 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 299 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 300 wait(0.001);
Dalaldeh 4:a4ac92030374 301 break;
Dalaldeh 4:a4ac92030374 302
Dalaldeh 4:a4ac92030374 303 }}
Dalaldeh 4:a4ac92030374 304
Dalaldeh 4:a4ac92030374 305
Dalaldeh 4:a4ac92030374 306 ///////////////////////////////////////////////////////***************Superviseur principal**************/////////////////////////////
Dalaldeh 4:a4ac92030374 307 void test ()
Dalaldeh 4:a4ac92030374 308 {
Dalaldeh 4:a4ac92030374 309 int i;
Dalaldeh 4:a4ac92030374 310 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 311 for(i=0; i<5; i++)
Dalaldeh 4:a4ac92030374 312 {
Dalaldeh 4:a4ac92030374 313
Dalaldeh 4:a4ac92030374 314 statecapt = (statecapt << 1) | seuil(tabsensor[i]);
Dalaldeh 4:a4ac92030374 315
Dalaldeh 4:a4ac92030374 316 }
Dalaldeh 4:a4ac92030374 317 vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+
Dalaldeh 4:a4ac92030374 318 (tabsensor[3]>600)*8+(tabsensor[4]>600)*16;
Dalaldeh 4:a4ac92030374 319
Dalaldeh 4:a4ac92030374 320 if(k==1)
Dalaldeh 4:a4ac92030374 321 {
Dalaldeh 4:a4ac92030374 322 if ((tabsensor[0]<400)&(tabsensor[4]<400))
Dalaldeh 4:a4ac92030374 323 {
Dalaldeh 4:a4ac92030374 324 if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400))
Dalaldeh 4:a4ac92030374 325 etat =2;
Dalaldeh 4:a4ac92030374 326 else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat =0;countt++;}
Dalaldeh 4:a4ac92030374 327 }
Dalaldeh 4:a4ac92030374 328 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<400)&(tabsensor[4]>600)) )
Dalaldeh 4:a4ac92030374 329 {
Dalaldeh 4:a4ac92030374 330 etat=1;countt++;
Dalaldeh 4:a4ac92030374 331 }
Dalaldeh 4:a4ac92030374 332 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>800)&(tabsensor[4]<100)) )
Dalaldeh 4:a4ac92030374 333 {
Dalaldeh 4:a4ac92030374 334 etat=4;countt++;
Dalaldeh 4:a4ac92030374 335 }
Dalaldeh 4:a4ac92030374 336 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix
Dalaldeh 4:a4ac92030374 337 {
Dalaldeh 4:a4ac92030374 338 etat=3 ;countt++;
Dalaldeh 4:a4ac92030374 339 }
Dalaldeh 4:a4ac92030374 340 }
Dalaldeh 4:a4ac92030374 341 switch (etat)
Dalaldeh 4:a4ac92030374 342 {
Dalaldeh 4:a4ac92030374 343 case 0:
Dalaldeh 4:a4ac92030374 344 m3pi.left_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 345 m3pi.right_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 346 wait(0.1);
Dalaldeh 4:a4ac92030374 347 m3pi.left_motor(speed);
Dalaldeh 4:a4ac92030374 348 m3pi.right_motor(-speed);
Dalaldeh 4:a4ac92030374 349 strcpy( aff,"back ");
Dalaldeh 4:a4ac92030374 350 wait(0.74);
Dalaldeh 4:a4ac92030374 351 PIDf(1);
Dalaldeh 4:a4ac92030374 352 wait(0.1);
Dalaldeh 4:a4ac92030374 353 save[countt-1]='B';
Dalaldeh 4:a4ac92030374 354 break;
Dalaldeh 4:a4ac92030374 355 case 1 :
Dalaldeh 4:a4ac92030374 356 strcpy( aff,"Right ");
Dalaldeh 4:a4ac92030374 357 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 358 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 359 wait(0.01);
Dalaldeh 4:a4ac92030374 360 m3pi.left_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 361 m3pi.right_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 362 wait(0.16);
Dalaldeh 4:a4ac92030374 363 save[countt-1]='R';
Dalaldeh 4:a4ac92030374 364 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 365 m3pi.right_motor(-(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 366 wait(0.42);
Dalaldeh 4:a4ac92030374 367 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 368 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 369 wait(0.1);
Dalaldeh 4:a4ac92030374 370 PIDf(1);
Dalaldeh 4:a4ac92030374 371 wait(0.001);
Dalaldeh 4:a4ac92030374 372 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 373 break;
Dalaldeh 4:a4ac92030374 374 case 2:
Dalaldeh 4:a4ac92030374 375 PIDf(1);
Dalaldeh 4:a4ac92030374 376 sprintf(aff,"%d",countt);//strcpy( aff,"bye nombre");
Dalaldeh 4:a4ac92030374 377 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 378 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 379 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 380 break;
Dalaldeh 4:a4ac92030374 381 case 3:
Dalaldeh 4:a4ac92030374 382 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 383 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 384 wait(0.01);
Dalaldeh 4:a4ac92030374 385 m3pi.left_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 386 m3pi.right_motor(1.2*(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 387 wait(0.16);
Dalaldeh 4:a4ac92030374 388 strcpy( aff,"t_forward ");
Dalaldeh 4:a4ac92030374 389 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 390 wait(0.02);
Dalaldeh 4:a4ac92030374 391 if ((tabsensor[0]>600)&(tabsensor[4]>200)&(tabsensor[2]>600)&(tabsensor[3]>600)&(tabsensor[1]>600))//the end
Dalaldeh 4:a4ac92030374 392 {
Dalaldeh 4:a4ac92030374 393 save[countt-1]='E';
Dalaldeh 4:a4ac92030374 394 m3pi.left_motor(2*0);
Dalaldeh 4:a4ac92030374 395 m3pi.right_motor(2*0);
Dalaldeh 4:a4ac92030374 396 cmd=false;
Dalaldeh 4:a4ac92030374 397 m3pi.leds(255);
Dalaldeh 4:a4ac92030374 398 wait(2);
Dalaldeh 4:a4ac92030374 399 strcpy( aff,"Optim ");
Dalaldeh 4:a4ac92030374 400 m3pi.leds(255);
Dalaldeh 4:a4ac92030374 401 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 402 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 403 wait(0.5);
Dalaldeh 4:a4ac92030374 404 m3pi.leds(255);
Dalaldeh 4:a4ac92030374 405 wait(0.5);
Dalaldeh 4:a4ac92030374 406 m3pi.leds(127);
Dalaldeh 4:a4ac92030374 407 wait(0.5);
Dalaldeh 4:a4ac92030374 408 m3pi.leds(63);
Dalaldeh 4:a4ac92030374 409 wait(0.5);
Dalaldeh 4:a4ac92030374 410 strcpy( aff,"2 ");
Dalaldeh 4:a4ac92030374 411 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 412 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 413 m3pi.leds(31);
Dalaldeh 4:a4ac92030374 414 wait(0.5);
Dalaldeh 4:a4ac92030374 415 m3pi.leds(15);
Dalaldeh 4:a4ac92030374 416 wait(0.5);
Dalaldeh 4:a4ac92030374 417 m3pi.leds(7);
Dalaldeh 4:a4ac92030374 418 strcpy( aff,"1 ");
Dalaldeh 4:a4ac92030374 419 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 420 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 421 m3pi.leds(3);
Dalaldeh 4:a4ac92030374 422 wait(0.5);
Dalaldeh 4:a4ac92030374 423 m3pi.leds(1);
Dalaldeh 4:a4ac92030374 424 wait(0.5);
Dalaldeh 4:a4ac92030374 425 strcpy( aff,"0 ");
Dalaldeh 4:a4ac92030374 426 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 427 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 428 wait(1);
Dalaldeh 4:a4ac92030374 429 m3pi.leds(0);
Dalaldeh 4:a4ac92030374 430 strcpy( aff,"GOOOOOOOOOO");
Dalaldeh 4:a4ac92030374 431 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 432 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 433 wait(1);
Dalaldeh 4:a4ac92030374 434 }
Dalaldeh 4:a4ac92030374 435 else
Dalaldeh 4:a4ac92030374 436 {
Dalaldeh 4:a4ac92030374 437 save[countt-1]='R';
Dalaldeh 4:a4ac92030374 438 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 439 m3pi.right_motor(-(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 440 wait(0.53);
Dalaldeh 4:a4ac92030374 441 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 442 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 443 wait(0.1);
Dalaldeh 4:a4ac92030374 444 PIDf(1);
Dalaldeh 4:a4ac92030374 445 wait(0.001);
Dalaldeh 4:a4ac92030374 446 strcpy( aff,"turned ");
Dalaldeh 4:a4ac92030374 447 }
Dalaldeh 4:a4ac92030374 448
Dalaldeh 4:a4ac92030374 449 break;
Dalaldeh 4:a4ac92030374 450 case 4:
Dalaldeh 4:a4ac92030374 451 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 452 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 453 wait(0.01);
Dalaldeh 4:a4ac92030374 454 m3pi.left_motor(2*(1.5/2)*0.8*speed);
Dalaldeh 4:a4ac92030374 455 m3pi.right_motor(2*(1.5/2)*0.8*speed);
Dalaldeh 4:a4ac92030374 456 wait(0.14);
Dalaldeh 4:a4ac92030374 457 strcpy( aff,"t_forward ");
Dalaldeh 4:a4ac92030374 458 PIDf(1);
Dalaldeh 4:a4ac92030374 459 m3pi.left_motor(0);
Dalaldeh 4:a4ac92030374 460 m3pi.right_motor(0);
Dalaldeh 4:a4ac92030374 461 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 462 save[countt-1]='W';
Dalaldeh 4:a4ac92030374 463 if (((tabsensor[0]<400)&(tabsensor[4]<400))&(tabsensor[2]>900))//forward
Dalaldeh 4:a4ac92030374 464 {
Dalaldeh 4:a4ac92030374 465 PIDf(1);
Dalaldeh 4:a4ac92030374 466 wait(0.001);
Dalaldeh 4:a4ac92030374 467 }
Dalaldeh 4:a4ac92030374 468 if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100))//turn left
Dalaldeh 4:a4ac92030374 469 {
Dalaldeh 4:a4ac92030374 470 save[countt-1]='L';
Dalaldeh 4:a4ac92030374 471 m3pi.left_motor(-(1.5/2)*speed);
Dalaldeh 4:a4ac92030374 472 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 473 wait(0.48);
Dalaldeh 4:a4ac92030374 474 m3pi.left_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 475 m3pi.right_motor((1.5/2)*speed);
Dalaldeh 4:a4ac92030374 476 wait(0.21);
Dalaldeh 4:a4ac92030374 477 }
Dalaldeh 4:a4ac92030374 478 break;
Dalaldeh 4:a4ac92030374 479 }}
Dalaldeh 4:a4ac92030374 480 ////////////////////////*********************************************/////////////////////////////////////////////////
Dalaldeh 4:a4ac92030374 481
Dalaldeh 4:a4ac92030374 482 volatile char flag10ms;
Dalaldeh 4:a4ac92030374 483
Dalaldeh 4:a4ac92030374 484 void inter1()
Dalaldeh 4:a4ac92030374 485 {
Dalaldeh 4:a4ac92030374 486 flag10ms=1;
Dalaldeh 4:a4ac92030374 487
Dalaldeh 4:a4ac92030374 488 }
Dalaldeh 4:a4ac92030374 489
Dalaldeh 4:a4ac92030374 490 int main()
Dalaldeh 4:a4ac92030374 491 {
Dalaldeh 4:a4ac92030374 492 k=1;
Dalaldeh 4:a4ac92030374 493 countt=0;
Dalaldeh 4:a4ac92030374 494 countt1=0;
Dalaldeh 4:a4ac92030374 495 cmd=1;
bouaziz 0:398afdd73d9e 496 resetxbee =0;
bouaziz 0:398afdd73d9e 497 wait(0.01);
bouaziz 0:398afdd73d9e 498 resetxbee =1;
Dalaldeh 4:a4ac92030374 499 statecapt=0;
bouaziz 0:398afdd73d9e 500 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 501 wait(1.);
Dalaldeh 4:a4ac92030374 502 tick1.attach(&inter1,0.01);
Dalaldeh 4:a4ac92030374 503 mu=1;
Dalaldeh 4:a4ac92030374 504 while(1)
Dalaldeh 4:a4ac92030374 505 {
Dalaldeh 4:a4ac92030374 506 if (1)
Dalaldeh 4:a4ac92030374 507 {
Dalaldeh 4:a4ac92030374 508 if(flag10ms==1)
Dalaldeh 4:a4ac92030374 509 {
Dalaldeh 4:a4ac92030374 510 flag10ms=0;
Dalaldeh 4:a4ac92030374 511 t.reset();
Dalaldeh 4:a4ac92030374 512 t.start();
Dalaldeh 4:a4ac92030374 513 m3pi.calibrated_sensors(tabsensor);
Dalaldeh 4:a4ac92030374 514 if(cmd)
Dalaldeh 4:a4ac92030374 515 {
Dalaldeh 4:a4ac92030374 516 test();
Dalaldeh 4:a4ac92030374 517 }
Dalaldeh 4:a4ac92030374 518 else
Dalaldeh 4:a4ac92030374 519 {
Dalaldeh 4:a4ac92030374 520 optim(save,countt);optim(save,countt);optim(save,countt);optim(save,countt);
Dalaldeh 4:a4ac92030374 521 test1();
Dalaldeh 4:a4ac92030374 522 }
Dalaldeh 4:a4ac92030374 523 t.stop();
Dalaldeh 4:a4ac92030374 524 v=t.read_us();
Dalaldeh 4:a4ac92030374 525 delai600ms++;
Dalaldeh 4:a4ac92030374 526 }
Dalaldeh 4:a4ac92030374 527 }
Dalaldeh 4:a4ac92030374 528 }
Dalaldeh 4:a4ac92030374 529
Dalaldeh 4:a4ac92030374 530 for (k=0 ;k<countt; k++ )
Dalaldeh 4:a4ac92030374 531 {
Dalaldeh 4:a4ac92030374 532 if(k<8)
Dalaldeh 4:a4ac92030374 533 {
Dalaldeh 4:a4ac92030374 534 aff[k]=save[k];
bouaziz 0:398afdd73d9e 535 }
Dalaldeh 4:a4ac92030374 536 else
Dalaldeh 4:a4ac92030374 537 {
Dalaldeh 4:a4ac92030374 538 aff1[k]=save1[k];
Dalaldeh 4:a4ac92030374 539 }
Dalaldeh 4:a4ac92030374 540
bouaziz 0:398afdd73d9e 541 }
Dalaldeh 4:a4ac92030374 542 m3pi.locate(0,1);
Dalaldeh 4:a4ac92030374 543 m3pi.printf(aff);
Dalaldeh 4:a4ac92030374 544
Dalaldeh 4:a4ac92030374 545 }
Dalaldeh 4:a4ac92030374 546