dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

Committer:
bouaziz
Date:
Mon Feb 11 09:46:33 2019 +0000
Revision:
8:6e2b5737c86c
Parent:
7:a7bf354ae965
deniere phase du TP laby

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouaziz 0:398afdd73d9e 1
bouaziz 0:398afdd73d9e 2 #include "mbed.h"
bouaziz 0:398afdd73d9e 3 #include "m3pi.h"
bouaziz 0:398afdd73d9e 4 #include "MSCFileSystem.h"
bouaziz 0:398afdd73d9e 5
bouaziz 0:398afdd73d9e 6 m3pi m3pi; // Initialise the m3pi
bouaziz 0:398afdd73d9e 7
bouaziz 0:398afdd73d9e 8 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
bouaziz 4:dcad3508bdfb 9 //MSCFileSystem fs("fs");
bouaziz 6:f7c38d092b97 10
bouaziz 7:a7bf354ae965 11 LocalFileSystem local("local");
bouaziz 7:a7bf354ae965 12
bouaziz 6:f7c38d092b97 13 Ticker tic1;
bouaziz 4:dcad3508bdfb 14 Timer tm1;
bouaziz 0:398afdd73d9e 15
bouaziz 0:398afdd73d9e 16 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 17
bouaziz 4:dcad3508bdfb 18 // all variables are float
bouaziz 4:dcad3508bdfb 19 #define D_TERM 0.0
bouaziz 4:dcad3508bdfb 20 #define I_TERMO 0.1
bouaziz 4:dcad3508bdfb 21 #define I_TERM 0.1
bouaziz 4:dcad3508bdfb 22 #define P_TERM 0.9
bouaziz 4:dcad3508bdfb 23 #define MAX 0.3
bouaziz 4:dcad3508bdfb 24 #define MIN -0.2
bouaziz 4:dcad3508bdfb 25 float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
bouaziz 4:dcad3508bdfb 26 float speed =0.3;
bouaziz 4:dcad3508bdfb 27
bouaziz 4:dcad3508bdfb 28
bouaziz 4:dcad3508bdfb 29 unsigned short tabsensor[5];
bouaziz 7:a7bf354ae965 30 #define seuil(x) (x>350 ? 1 : 0)
bouaziz 4:dcad3508bdfb 31 unsigned char statcapt;
bouaziz 7:a7bf354ae965 32 unsigned char LFRstat, LFRvect;
bouaziz 5:0f4a460521be 33
bouaziz 5:0f4a460521be 34 // fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet
bouaziz 5:0f4a460521be 35 // seuls 5 bits sont utiles
bouaziz 5:0f4a460521be 36 // Vérifier l'ordre des bits sur la variable retrounée stat
bouaziz 5:0f4a460521be 37 // bit4 à bit0 de stat sachant que bit2 c'est le capteur milieu
bouaziz 5:0f4a460521be 38 unsigned char lecture_captsol(unsigned short *tab){
bouaziz 5:0f4a460521be 39 unsigned char stat=0;
bouaziz 5:0f4a460521be 40 m3pi.calibrated_sensors(tab);
bouaziz 5:0f4a460521be 41 for(unsigned short ii=0;ii<5;ii++){
bouaziz 5:0f4a460521be 42 stat = (stat <<1) | seuil(tab[ii]);
bouaziz 4:dcad3508bdfb 43 }
bouaziz 7:a7bf354ae965 44 LFRstat = (((stat&0x04)==0x04)||((stat&0x03)==0x02)||((stat&0x18)==0x08) ? 0x02 : 0) | (((stat&0x03)==0x03) ? 0x01 : 0)|(((stat&0x18)==0x18) ? 0x04 : 0);
bouaziz 5:0f4a460521be 45 return stat;
bouaziz 5:0f4a460521be 46 }
bouaziz 5:0f4a460521be 47
bouaziz 5:0f4a460521be 48 // Asservissement PID en flottant du robot.
bouaziz 5:0f4a460521be 49 // remplacer certains commentaires de limiteurs llimit checks on motor control
bouaziz 5:0f4a460521be 50 void PIDf(){
bouaziz 5:0f4a460521be 51
bouaziz 5:0f4a460521be 52 // Get the position of the line.
bouaziz 5:0f4a460521be 53 current_pos_of_line = m3pi.line_position();
bouaziz 5:0f4a460521be 54 proportional = current_pos_of_line;
bouaziz 5:0f4a460521be 55 // Compute the derivative
bouaziz 5:0f4a460521be 56 derivative = current_pos_of_line - previous_pos_of_line;
bouaziz 5:0f4a460521be 57 // Compute the integral
bouaziz 5:0f4a460521be 58 integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
bouaziz 5:0f4a460521be 59 // Remember the last position.
bouaziz 5:0f4a460521be 60 previous_pos_of_line = current_pos_of_line;
bouaziz 5:0f4a460521be 61 // Compute the power
bouaziz 5:0f4a460521be 62 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
bouaziz 5:0f4a460521be 63 // Compute new speeds
bouaziz 5:0f4a460521be 64 right = speed-(power*MAX);
bouaziz 5:0f4a460521be 65 left = speed+(power*MAX);
bouaziz 5:0f4a460521be 66 // limit checks on motor control
bouaziz 5:0f4a460521be 67 //MIN <right < MAX
bouaziz 5:0f4a460521be 68 // MIN <left < MAX
bouaziz 7:a7bf354ae965 69 left = (left>MAX ? MAX :(left<MIN ? MIN : left));
bouaziz 5:0f4a460521be 70 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
bouaziz 5:0f4a460521be 71 // send command to motors
bouaziz 5:0f4a460521be 72 m3pi.left_motor(left);
bouaziz 5:0f4a460521be 73 m3pi.right_motor(right);
bouaziz 4:dcad3508bdfb 74 }
bouaziz 4:dcad3508bdfb 75
bouaziz 4:dcad3508bdfb 76 volatile char flag10ms;
bouaziz 4:dcad3508bdfb 77 void inter1(){
bouaziz 4:dcad3508bdfb 78 flag10ms=1;
bouaziz 4:dcad3508bdfb 79 }
bouaziz 4:dcad3508bdfb 80
bouaziz 5:0f4a460521be 81 int v,count;
bouaziz 4:dcad3508bdfb 82 unsigned char delai600ms;
bouaziz 4:dcad3508bdfb 83 char chain[10];
bouaziz 6:f7c38d092b97 84 volatile char flag1sec,flagmesure;
bouaziz 6:f7c38d092b97 85 char autom;
bouaziz 6:f7c38d092b97 86 unsigned int valtimer;
bouaziz 6:f7c38d092b97 87
bouaziz 6:f7c38d092b97 88 void automate(){
bouaziz 6:f7c38d092b97 89
bouaziz 6:f7c38d092b97 90 switch(autom){
bouaziz 6:f7c38d092b97 91 case 0 : m3pi.forward(0.3);
bouaziz 6:f7c38d092b97 92 autom=1;
bouaziz 6:f7c38d092b97 93 break;
bouaziz 6:f7c38d092b97 94 case 1 :
bouaziz 6:f7c38d092b97 95 if((statcapt&0x11)==0x11){
bouaziz 6:f7c38d092b97 96 autom=2;
bouaziz 6:f7c38d092b97 97 }
bouaziz 6:f7c38d092b97 98 break;
bouaziz 6:f7c38d092b97 99 case 2 :
bouaziz 6:f7c38d092b97 100 if((statcapt&0x11)==0x00){
bouaziz 6:f7c38d092b97 101 autom=3;
bouaziz 6:f7c38d092b97 102 tm1.start();
bouaziz 6:f7c38d092b97 103 }
bouaziz 6:f7c38d092b97 104 break;
bouaziz 6:f7c38d092b97 105 case 3 :
bouaziz 6:f7c38d092b97 106 if((statcapt&0x11)==0x11){
bouaziz 6:f7c38d092b97 107 autom=4;
bouaziz 6:f7c38d092b97 108 m3pi.stop();
bouaziz 6:f7c38d092b97 109 tm1.stop();
bouaziz 6:f7c38d092b97 110 valtimer=tm1.read_us();
bouaziz 6:f7c38d092b97 111 flagmesure=1;
bouaziz 6:f7c38d092b97 112 }
bouaziz 6:f7c38d092b97 113 break;
bouaziz 6:f7c38d092b97 114 default:
bouaziz 6:f7c38d092b97 115 break;
bouaziz 6:f7c38d092b97 116 }
bouaziz 6:f7c38d092b97 117 }
bouaziz 5:0f4a460521be 118
bouaziz 6:f7c38d092b97 119 char sensrotation=0; // 0 1 Left 2 Right
bouaziz 6:f7c38d092b97 120 char rotation_flag=0;
bouaziz 7:a7bf354ae965 121 static unsigned char automr=0;
bouaziz 6:f7c38d092b97 122 void automrotation(){
bouaziz 7:a7bf354ae965 123
bouaziz 6:f7c38d092b97 124 static unsigned short vtime;
bouaziz 6:f7c38d092b97 125 switch(automr){
bouaziz 6:f7c38d092b97 126 case 0 : m3pi.stop();
bouaziz 6:f7c38d092b97 127 automr=1;
bouaziz 6:f7c38d092b97 128 vtime=0;
bouaziz 6:f7c38d092b97 129 break;
bouaziz 6:f7c38d092b97 130 case 1 :vtime++;
bouaziz 6:f7c38d092b97 131 if(vtime>=25){
bouaziz 6:f7c38d092b97 132 switch(sensrotation){
bouaziz 6:f7c38d092b97 133 case 2 :
bouaziz 6:f7c38d092b97 134 m3pi.right(0.3);
bouaziz 6:f7c38d092b97 135 break;
bouaziz 6:f7c38d092b97 136 case 1 :
bouaziz 6:f7c38d092b97 137 m3pi.left(0.3);
bouaziz 6:f7c38d092b97 138 break;
bouaziz 6:f7c38d092b97 139 default :
bouaziz 6:f7c38d092b97 140 automr=0;
bouaziz 6:f7c38d092b97 141 break;
bouaziz 6:f7c38d092b97 142 }
bouaziz 6:f7c38d092b97 143 automr=2;
bouaziz 6:f7c38d092b97 144 }
bouaziz 6:f7c38d092b97 145 break;
bouaziz 6:f7c38d092b97 146 case 2 :
bouaziz 6:f7c38d092b97 147 if(sensrotation){
bouaziz 6:f7c38d092b97 148 if((statcapt&1)&& (sensrotation==2)){
bouaziz 6:f7c38d092b97 149 automr=3;
bouaziz 6:f7c38d092b97 150 }
bouaziz 7:a7bf354ae965 151 }else{
bouaziz 6:f7c38d092b97 152 if((statcapt&0x10) && (sensrotation==1)){
bouaziz 6:f7c38d092b97 153 automr=3;
bouaziz 6:f7c38d092b97 154 }
bouaziz 6:f7c38d092b97 155 }
bouaziz 6:f7c38d092b97 156 break;
bouaziz 6:f7c38d092b97 157 case 3 :
bouaziz 6:f7c38d092b97 158 if( ((sensrotation==2)&&(statcapt&0x06))||((sensrotation==1)&&(statcapt&0x0C)) ) {
bouaziz 6:f7c38d092b97 159 m3pi.stop();
bouaziz 6:f7c38d092b97 160 vtime=0;
bouaziz 6:f7c38d092b97 161 automr=4;
bouaziz 6:f7c38d092b97 162 }
bouaziz 6:f7c38d092b97 163 break;
bouaziz 6:f7c38d092b97 164 case 4 :vtime++;
bouaziz 6:f7c38d092b97 165 if(vtime>=30){
bouaziz 6:f7c38d092b97 166 rotation_flag=1;
bouaziz 6:f7c38d092b97 167 automr=0;
bouaziz 6:f7c38d092b97 168 }
bouaziz 6:f7c38d092b97 169 break;
bouaziz 6:f7c38d092b97 170 default:
bouaziz 6:f7c38d092b97 171 automr=0;
bouaziz 6:f7c38d092b97 172 break;
bouaziz 6:f7c38d092b97 173 }
bouaziz 6:f7c38d092b97 174 }
bouaziz 6:f7c38d092b97 175
bouaziz 6:f7c38d092b97 176 unsigned char inver;
bouaziz 6:f7c38d092b97 177 char trotate[2] = {1,2};
bouaziz 6:f7c38d092b97 178 char indexrotate=0;
bouaziz 7:a7bf354ae965 179
bouaziz 7:a7bf354ae965 180 #define MAXSAVE 4000
bouaziz 7:a7bf354ae965 181 unsigned char sauvetab[MAXSAVE][3];
bouaziz 7:a7bf354ae965 182
bouaziz 8:6e2b5737c86c 183 unsigned char texttab[300];
bouaziz 8:6e2b5737c86c 184 unsigned char TXT[20]={"FLFLLRLLRR"};
bouaziz 8:6e2b5737c86c 185 unsigned short intercount,index;
bouaziz 7:a7bf354ae965 186
bouaziz 7:a7bf354ae965 187 unsigned lvwait,lwait2;
bouaziz 8:6e2b5737c86c 188 char mess[20];
bouaziz 8:6e2b5737c86c 189
bouaziz 8:6e2b5737c86c 190 char rejoue(){
bouaziz 8:6e2b5737c86c 191 static unsigned char automain2=0;
bouaziz 8:6e2b5737c86c 192 switch(automain2){
bouaziz 8:6e2b5737c86c 193 case 0 : PIDf();
bouaziz 8:6e2b5737c86c 194 switch(LFRvect) {
bouaziz 8:6e2b5737c86c 195 case 0x77 :
bouaziz 8:6e2b5737c86c 196 automain2=37;
bouaziz 8:6e2b5737c86c 197 lvwait=0;
bouaziz 8:6e2b5737c86c 198 break;
bouaziz 8:6e2b5737c86c 199 case 0x00 : // pas bon
bouaziz 8:6e2b5737c86c 200 m3pi.stop();
bouaziz 8:6e2b5737c86c 201 m3pi.cls();
bouaziz 8:6e2b5737c86c 202 sprintf(mess,"ERR0:%u",index);
bouaziz 8:6e2b5737c86c 203 m3pi.print(mess,strlen(mess));
bouaziz 8:6e2b5737c86c 204 while(1);
bouaziz 8:6e2b5737c86c 205 //Indiquer probleme simplification
bouaziz 8:6e2b5737c86c 206 break;
bouaziz 8:6e2b5737c86c 207 case 0x33 :
bouaziz 8:6e2b5737c86c 208 automain2= 13;
bouaziz 8:6e2b5737c86c 209
bouaziz 8:6e2b5737c86c 210 break;
bouaziz 8:6e2b5737c86c 211 case 0x66 :
bouaziz 8:6e2b5737c86c 212 automain2= 13;
bouaziz 8:6e2b5737c86c 213
bouaziz 8:6e2b5737c86c 214 break;
bouaziz 8:6e2b5737c86c 215 case 0x22 :
bouaziz 8:6e2b5737c86c 216
bouaziz 8:6e2b5737c86c 217 // break;
bouaziz 8:6e2b5737c86c 218 default :
bouaziz 8:6e2b5737c86c 219 break;
bouaziz 8:6e2b5737c86c 220 }
bouaziz 8:6e2b5737c86c 221 break;
bouaziz 8:6e2b5737c86c 222 case 2 : // fin rotation droite
bouaziz 8:6e2b5737c86c 223 if(LFRvect==0x22){
bouaziz 8:6e2b5737c86c 224 m3pi.stop();
bouaziz 8:6e2b5737c86c 225 automain2=0;
bouaziz 8:6e2b5737c86c 226 }
bouaziz 8:6e2b5737c86c 227 break;
bouaziz 8:6e2b5737c86c 228 case 13 : //inter droite ou tout droit
bouaziz 8:6e2b5737c86c 229 PIDf();
bouaziz 8:6e2b5737c86c 230 if((LFRvect ==0x00) || (LFRvect ==0x22)) {
bouaziz 8:6e2b5737c86c 231 switch(TXT[index]){
bouaziz 8:6e2b5737c86c 232 case 'F' :
bouaziz 8:6e2b5737c86c 233 index++;
bouaziz 8:6e2b5737c86c 234 automain2 =0;
bouaziz 8:6e2b5737c86c 235 break;
bouaziz 8:6e2b5737c86c 236 case 'R' :
bouaziz 8:6e2b5737c86c 237 //m3pi.stop();
bouaziz 8:6e2b5737c86c 238 lvwait=0;
bouaziz 8:6e2b5737c86c 239 index++;
bouaziz 8:6e2b5737c86c 240 if(LFRvect == 0x00){
bouaziz 8:6e2b5737c86c 241 m3pi.right(0.2);
bouaziz 8:6e2b5737c86c 242 automain2=2;
bouaziz 8:6e2b5737c86c 243 }else{
bouaziz 8:6e2b5737c86c 244 // alors c'est 0x22
bouaziz 8:6e2b5737c86c 245 automain2 =14; //on tourne à droite }
bouaziz 8:6e2b5737c86c 246 break;
bouaziz 8:6e2b5737c86c 247 case 'L' :
bouaziz 8:6e2b5737c86c 248 lvwait=0;
bouaziz 8:6e2b5737c86c 249 index++;
bouaziz 8:6e2b5737c86c 250 if(LFRvect == 0x00){
bouaziz 8:6e2b5737c86c 251 m3pi.left(0.2);
bouaziz 8:6e2b5737c86c 252 automain2=2;
bouaziz 8:6e2b5737c86c 253 }else{
bouaziz 8:6e2b5737c86c 254 automain2 =17; //on tourne à droite
bouaziz 8:6e2b5737c86c 255 } }
bouaziz 8:6e2b5737c86c 256 break;
bouaziz 8:6e2b5737c86c 257 default :
bouaziz 8:6e2b5737c86c 258 break;
bouaziz 8:6e2b5737c86c 259 }
bouaziz 8:6e2b5737c86c 260 }
bouaziz 8:6e2b5737c86c 261 break;
bouaziz 8:6e2b5737c86c 262
bouaziz 8:6e2b5737c86c 263 case 14 :
bouaziz 8:6e2b5737c86c 264 lvwait++;
bouaziz 8:6e2b5737c86c 265 if(lvwait>(60/10)){
bouaziz 8:6e2b5737c86c 266 m3pi.stop();
bouaziz 8:6e2b5737c86c 267 automain=15;
bouaziz 8:6e2b5737c86c 268 lvwait=0;
bouaziz 8:6e2b5737c86c 269 }
bouaziz 8:6e2b5737c86c 270 break;
bouaziz 8:6e2b5737c86c 271
bouaziz 8:6e2b5737c86c 272 case 15 :
bouaziz 8:6e2b5737c86c 273 lvwait++;
bouaziz 8:6e2b5737c86c 274 if(lvwait>(500/10)){
bouaziz 8:6e2b5737c86c 275 m3pi.right(0.2);
bouaziz 8:6e2b5737c86c 276 automain=16;
bouaziz 8:6e2b5737c86c 277 lvwait=0;
bouaziz 8:6e2b5737c86c 278 }
bouaziz 8:6e2b5737c86c 279 break;
bouaziz 8:6e2b5737c86c 280 case 16 :
bouaziz 8:6e2b5737c86c 281 if((LFRvect&0x22)==0){
bouaziz 8:6e2b5737c86c 282 automain=2;
bouaziz 8:6e2b5737c86c 283 }
bouaziz 8:6e2b5737c86c 284 break;
bouaziz 8:6e2b5737c86c 285 case 17 :
bouaziz 8:6e2b5737c86c 286 lvwait++;
bouaziz 8:6e2b5737c86c 287 if(lvwait>(60/10)){
bouaziz 8:6e2b5737c86c 288 m3pi.stop();
bouaziz 8:6e2b5737c86c 289 automain2=18;
bouaziz 8:6e2b5737c86c 290 lvwait=0;
bouaziz 8:6e2b5737c86c 291 }
bouaziz 8:6e2b5737c86c 292 break;
bouaziz 8:6e2b5737c86c 293
bouaziz 8:6e2b5737c86c 294 case 18 :
bouaziz 8:6e2b5737c86c 295 lvwait++;
bouaziz 8:6e2b5737c86c 296 if(lvwait>(500/10)){
bouaziz 8:6e2b5737c86c 297 m3pi.left(0.2);
bouaziz 8:6e2b5737c86c 298 automain2=16;
bouaziz 8:6e2b5737c86c 299 lvwait=0;
bouaziz 8:6e2b5737c86c 300 }
bouaziz 8:6e2b5737c86c 301 break;
bouaziz 8:6e2b5737c86c 302 case 37 :
bouaziz 8:6e2b5737c86c 303 PIDf();
bouaziz 8:6e2b5737c86c 304 lvwait++;
bouaziz 8:6e2b5737c86c 305 if(LFRvect==0x77){
bouaziz 8:6e2b5737c86c 306 if((lvwait>15)){
bouaziz 8:6e2b5737c86c 307 m3pi.stop();
bouaziz 8:6e2b5737c86c 308 automain2=38;
bouaziz 8:6e2b5737c86c 309
bouaziz 8:6e2b5737c86c 310 }
bouaziz 8:6e2b5737c86c 311 }else{
bouaziz 8:6e2b5737c86c 312
bouaziz 8:6e2b5737c86c 313 automain2= 13;
bouaziz 8:6e2b5737c86c 314 lvwait=0;
bouaziz 8:6e2b5737c86c 315 }
bouaziz 8:6e2b5737c86c 316 break;
bouaziz 8:6e2b5737c86c 317 case 38 :
bouaziz 8:6e2b5737c86c 318 myleds=0x0F;
bouaziz 8:6e2b5737c86c 319 return 1;
bouaziz 8:6e2b5737c86c 320 default :
bouaziz 8:6e2b5737c86c 321 break;
bouaziz 8:6e2b5737c86c 322 }
bouaziz 8:6e2b5737c86c 323 return 0;
bouaziz 8:6e2b5737c86c 324 }
bouaziz 8:6e2b5737c86c 325
bouaziz 8:6e2b5737c86c 326
bouaziz 8:6e2b5737c86c 327 char explore(){
bouaziz 8:6e2b5737c86c 328 static unsigned char automain=0;
bouaziz 8:6e2b5737c86c 329 switch(automain){
bouaziz 8:6e2b5737c86c 330 case 0 : PIDf();
bouaziz 8:6e2b5737c86c 331 switch(LFRvect) {
bouaziz 8:6e2b5737c86c 332 case 0x77 :
bouaziz 8:6e2b5737c86c 333 automain=37;
bouaziz 8:6e2b5737c86c 334 lvwait=0;
bouaziz 8:6e2b5737c86c 335 break;
bouaziz 8:6e2b5737c86c 336 case 0x00 :
bouaziz 8:6e2b5737c86c 337 //m3pi.stop();
bouaziz 8:6e2b5737c86c 338 automain= 3;
bouaziz 8:6e2b5737c86c 339 lvwait=0;
bouaziz 8:6e2b5737c86c 340 texttab[intercount++]='B';
bouaziz 8:6e2b5737c86c 341 break;
bouaziz 8:6e2b5737c86c 342 case 0x33 :
bouaziz 8:6e2b5737c86c 343 automain= 13;
bouaziz 8:6e2b5737c86c 344 texttab[intercount++]='R';
bouaziz 8:6e2b5737c86c 345 break;
bouaziz 8:6e2b5737c86c 346 case 0x66 :
bouaziz 8:6e2b5737c86c 347 automain= 26;
bouaziz 8:6e2b5737c86c 348
bouaziz 8:6e2b5737c86c 349 break;
bouaziz 8:6e2b5737c86c 350 case 0x22 :
bouaziz 8:6e2b5737c86c 351
bouaziz 8:6e2b5737c86c 352 // break;
bouaziz 8:6e2b5737c86c 353 default :
bouaziz 8:6e2b5737c86c 354 break;
bouaziz 8:6e2b5737c86c 355 }
bouaziz 8:6e2b5737c86c 356 break;
bouaziz 8:6e2b5737c86c 357 case 3 : lvwait++;
bouaziz 8:6e2b5737c86c 358 if(lvwait>(60/10)){
bouaziz 8:6e2b5737c86c 359 m3pi.stop();
bouaziz 8:6e2b5737c86c 360 automain=1;
bouaziz 8:6e2b5737c86c 361 }
bouaziz 8:6e2b5737c86c 362 break;
bouaziz 8:6e2b5737c86c 363 case 1 : //lancer STOP avec rotation à droite
bouaziz 8:6e2b5737c86c 364 lvwait++;
bouaziz 8:6e2b5737c86c 365 if(lvwait>(500/10)){
bouaziz 8:6e2b5737c86c 366 m3pi.right(0.2);
bouaziz 8:6e2b5737c86c 367 automain=2;
bouaziz 8:6e2b5737c86c 368 }
bouaziz 8:6e2b5737c86c 369 break;
bouaziz 8:6e2b5737c86c 370 case 2 : // jattends pour STOPPER
bouaziz 8:6e2b5737c86c 371 if(LFRvect==0x22){
bouaziz 8:6e2b5737c86c 372 m3pi.stop();
bouaziz 8:6e2b5737c86c 373 automain=0;
bouaziz 8:6e2b5737c86c 374 }
bouaziz 8:6e2b5737c86c 375 break;
bouaziz 8:6e2b5737c86c 376 case 13 : //inter droite ou tout droit
bouaziz 8:6e2b5737c86c 377 PIDf();
bouaziz 8:6e2b5737c86c 378 switch(LFRvect){
bouaziz 8:6e2b5737c86c 379 case 0x00 :
bouaziz 8:6e2b5737c86c 380 m3pi.stop();
bouaziz 8:6e2b5737c86c 381 lvwait=0;
bouaziz 8:6e2b5737c86c 382 automain =1;
bouaziz 8:6e2b5737c86c 383 case 0x22 :
bouaziz 8:6e2b5737c86c 384 //m3pi.stop();
bouaziz 8:6e2b5737c86c 385 lvwait=0;
bouaziz 8:6e2b5737c86c 386 automain =14;
bouaziz 8:6e2b5737c86c 387 default :
bouaziz 8:6e2b5737c86c 388 break;
bouaziz 8:6e2b5737c86c 389 }
bouaziz 8:6e2b5737c86c 390 break;
bouaziz 8:6e2b5737c86c 391
bouaziz 8:6e2b5737c86c 392 case 14 :
bouaziz 8:6e2b5737c86c 393 lvwait++;
bouaziz 8:6e2b5737c86c 394 if(lvwait>(60/10)){
bouaziz 8:6e2b5737c86c 395 m3pi.stop();
bouaziz 8:6e2b5737c86c 396 automain=15;
bouaziz 8:6e2b5737c86c 397 lvwait=0;
bouaziz 8:6e2b5737c86c 398 }
bouaziz 8:6e2b5737c86c 399 break;
bouaziz 8:6e2b5737c86c 400
bouaziz 8:6e2b5737c86c 401 case 15 :
bouaziz 8:6e2b5737c86c 402 lvwait++;
bouaziz 8:6e2b5737c86c 403 if(lvwait>(500/10)){
bouaziz 8:6e2b5737c86c 404 m3pi.right(0.2);
bouaziz 8:6e2b5737c86c 405 automain=16;
bouaziz 8:6e2b5737c86c 406 lvwait=0;
bouaziz 8:6e2b5737c86c 407 }
bouaziz 8:6e2b5737c86c 408 break;
bouaziz 8:6e2b5737c86c 409 case 16 :
bouaziz 8:6e2b5737c86c 410 if((LFRvect&0x22)==0){
bouaziz 8:6e2b5737c86c 411 automain=2;
bouaziz 8:6e2b5737c86c 412 }
bouaziz 8:6e2b5737c86c 413 break;
bouaziz 8:6e2b5737c86c 414 case 26 : // inter gauche ou tout droit
bouaziz 8:6e2b5737c86c 415 PIDf();
bouaziz 8:6e2b5737c86c 416 switch(LFRvect){
bouaziz 8:6e2b5737c86c 417 case 0x00 :
bouaziz 8:6e2b5737c86c 418 // m3pi.stop();
bouaziz 8:6e2b5737c86c 419 lvwait=0;
bouaziz 8:6e2b5737c86c 420 automain =27;
bouaziz 8:6e2b5737c86c 421 texttab[intercount++]='L';
bouaziz 8:6e2b5737c86c 422 break;
bouaziz 8:6e2b5737c86c 423 case 0x22 : texttab[intercount++]='F';
bouaziz 8:6e2b5737c86c 424 automain =0;
bouaziz 8:6e2b5737c86c 425 break;
bouaziz 8:6e2b5737c86c 426 default :
bouaziz 8:6e2b5737c86c 427 break;
bouaziz 8:6e2b5737c86c 428 }
bouaziz 8:6e2b5737c86c 429 break;
bouaziz 8:6e2b5737c86c 430 case 27 :
bouaziz 8:6e2b5737c86c 431 lvwait++;
bouaziz 8:6e2b5737c86c 432 if(lvwait>(50/10)){
bouaziz 8:6e2b5737c86c 433 m3pi.stop();
bouaziz 8:6e2b5737c86c 434 automain=28;
bouaziz 8:6e2b5737c86c 435 lvwait=0;
bouaziz 8:6e2b5737c86c 436 }
bouaziz 8:6e2b5737c86c 437 break;
bouaziz 8:6e2b5737c86c 438 case 28 :
bouaziz 8:6e2b5737c86c 439 lvwait++;
bouaziz 8:6e2b5737c86c 440 if(lvwait>(500/10)){
bouaziz 8:6e2b5737c86c 441 m3pi.left(0.2);
bouaziz 8:6e2b5737c86c 442 automain=2;
bouaziz 8:6e2b5737c86c 443 }
bouaziz 8:6e2b5737c86c 444 break;
bouaziz 8:6e2b5737c86c 445 case 29 :
bouaziz 8:6e2b5737c86c 446 if(lvwait++>2){
bouaziz 8:6e2b5737c86c 447 m3pi.stop();
bouaziz 8:6e2b5737c86c 448 automain=0;
bouaziz 8:6e2b5737c86c 449 }
bouaziz 8:6e2b5737c86c 450 break;
bouaziz 8:6e2b5737c86c 451 case 37 :
bouaziz 8:6e2b5737c86c 452 PIDf();
bouaziz 8:6e2b5737c86c 453 lvwait++;
bouaziz 8:6e2b5737c86c 454 if(LFRvect==0x77){
bouaziz 8:6e2b5737c86c 455 if((lvwait>15)){
bouaziz 8:6e2b5737c86c 456 m3pi.stop();
bouaziz 8:6e2b5737c86c 457 automain=38;
bouaziz 8:6e2b5737c86c 458 }
bouaziz 8:6e2b5737c86c 459 }else{
bouaziz 8:6e2b5737c86c 460 texttab[intercount++]='R';
bouaziz 8:6e2b5737c86c 461 automain= 13;
bouaziz 8:6e2b5737c86c 462 lvwait=0;
bouaziz 8:6e2b5737c86c 463 }
bouaziz 8:6e2b5737c86c 464 break;
bouaziz 8:6e2b5737c86c 465 case 38 :
bouaziz 8:6e2b5737c86c 466 myleds=0x0F;
bouaziz 8:6e2b5737c86c 467 return 1;
bouaziz 8:6e2b5737c86c 468 default :
bouaziz 8:6e2b5737c86c 469 break;
bouaziz 8:6e2b5737c86c 470 }
bouaziz 8:6e2b5737c86c 471 return 0;
bouaziz 8:6e2b5737c86c 472 }
bouaziz 8:6e2b5737c86c 473
bouaziz 8:6e2b5737c86c 474
bouaziz 0:398afdd73d9e 475 int main() {
bouaziz 7:a7bf354ae965 476 unsigned short i=0,ii;
bouaziz 8:6e2b5737c86c 477 unsigned char supauto=2;
bouaziz 7:a7bf354ae965 478 myleds=0x0;
bouaziz 7:a7bf354ae965 479 FILE *p= fopen("/local/tt.txt","w");
bouaziz 7:a7bf354ae965 480 if(p!=0) {
bouaziz 7:a7bf354ae965 481 myleds=1;
bouaziz 7:a7bf354ae965 482 }
bouaziz 6:f7c38d092b97 483 wait(1.);
bouaziz 0:398afdd73d9e 484 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 485 wait(1.);
bouaziz 6:f7c38d092b97 486 //3pi.cls();
bouaziz 7:a7bf354ae965 487
bouaziz 5:0f4a460521be 488 tic1.attach(&inter1,0.01);
bouaziz 6:f7c38d092b97 489 tm1.reset();
bouaziz 4:dcad3508bdfb 490 // fprintf(p,"ecrire dans la cle USB\r\n");
bouaziz 4:dcad3508bdfb 491 // fclose(p);
bouaziz 0:398afdd73d9e 492
bouaziz 0:398afdd73d9e 493 while(1) {
bouaziz 5:0f4a460521be 494 // exemple de code
bouaziz 5:0f4a460521be 495 if(flag10ms){
bouaziz 6:f7c38d092b97 496 // myleds=0;
bouaziz 4:dcad3508bdfb 497 flag10ms=0;
bouaziz 5:0f4a460521be 498 statcapt=lecture_captsol(tabsensor);
bouaziz 7:a7bf354ae965 499 LFRvect = (LFRvect <<4) + LFRstat;
bouaziz 8:6e2b5737c86c 500 /* if(i<MAXSAVE){
bouaziz 7:a7bf354ae965 501 sauvetab[i][0]=automain;
bouaziz 7:a7bf354ae965 502 sauvetab[i][1]=LFRvect;
bouaziz 7:a7bf354ae965 503 sauvetab[i][2]=statcapt;
bouaziz 7:a7bf354ae965 504 i++;
bouaziz 8:6e2b5737c86c 505 }*/
bouaziz 8:6e2b5737c86c 506 switch(supauto){
bouaziz 8:6e2b5737c86c 507 case 0 :
bouaziz 8:6e2b5737c86c 508 if(explore()){
bouaziz 8:6e2b5737c86c 509 // simplification
bouaziz 8:6e2b5737c86c 510 //attendre
bouaziz 7:a7bf354ae965 511 lvwait=0;
bouaziz 8:6e2b5737c86c 512 supauto=1;
bouaziz 8:6e2b5737c86c 513 break;
bouaziz 8:6e2b5737c86c 514 case 1 :
bouaziz 8:6e2b5737c86c 515 lvwait++;
bouaziz 8:6e2b5737c86c 516 if(lvwait>=1500){
bouaziz 8:6e2b5737c86c 517 myleds=0;
bouaziz 8:6e2b5737c86c 518 supauto=2;
bouaziz 8:6e2b5737c86c 519 break;
bouaziz 8:6e2b5737c86c 520 case 2 :
bouaziz 8:6e2b5737c86c 521 if(rejoue()){
bouaziz 8:6e2b5737c86c 522 supauto=3;
bouaziz 8:6e2b5737c86c 523 }
bouaziz 8:6e2b5737c86c 524 break;
bouaziz 8:6e2b5737c86c 525 case 3 :
bouaziz 8:6e2b5737c86c 526 myleds(0x06);
bouaziz 8:6e2b5737c86c 527 default :
bouaziz 8:6e2b5737c86c 528 // se mettre en arret et afficher ereur
bouaziz 8:6e2b5737c86c 529 break;
bouaziz 8:6e2b5737c86c 530
bouaziz 5:0f4a460521be 531 }
bouaziz 6:f7c38d092b97 532
bouaziz 8:6e2b5737c86c 533
bouaziz 5:0f4a460521be 534 }
bouaziz 8:6e2b5737c86c 535 /* if(i>=MAXSAVE) {
bouaziz 7:a7bf354ae965 536 m3pi.stop();
bouaziz 7:a7bf354ae965 537 texttab[intercount++]=0;
bouaziz 7:a7bf354ae965 538 fprintf(p,"%s\n",texttab);
bouaziz 7:a7bf354ae965 539 for(i=0;i<MAXSAVE;i++){
bouaziz 7:a7bf354ae965 540 fprintf(p,"%3u %02x %02x\n",(unsigned)sauvetab[i][0],(unsigned)sauvetab[i][1],(unsigned)sauvetab[i][2]);
bouaziz 7:a7bf354ae965 541 }
bouaziz 7:a7bf354ae965 542 myleds=0x0f;
bouaziz 7:a7bf354ae965 543 fclose(p);
bouaziz 7:a7bf354ae965 544 while(1);
bouaziz 8:6e2b5737c86c 545 }*/
bouaziz 7:a7bf354ae965 546
bouaziz 7:a7bf354ae965 547
bouaziz 0:398afdd73d9e 548 }
bouaziz 4:dcad3508bdfb 549
bouaziz 0:398afdd73d9e 550 }