Revision 380 branch/bacher/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
123 123

  
124 124
int cons=450; //contient la consigne
125 125
int retour=0; //contient la valeur retourner par le p?riph
126
int k=3; //contient le gain du correcteur de la boucle de r?gulation de la tourelle
126
int k=5; //contient le gain du correcteur de la boucle de r?gulation de la tourelle
127 127
int k2=1; //contient le gain du correcteur de l'angle des roues
128 128
int vitesse=0; //vitesse en ligne droite
129 129
int vitesse_virage=0; //vitesse dans les virages
......
141 141
int num_piste=0; //contient l'identifiant de la piste
142 142
int col_feu=0; //contient la couleur du feu
143 143
int mode=0; //mode de ralentissemennt dans les virages
144
int compte_tour=0;
145
int i_tour=0;
146
int stop=0;
147
int i_urgence=0;
148
int s_urgence=0;
149
int cu;
150

  
144 151
FLGPTN flag_capt;
145 152
FLGPTN flag_pist;
146 153

  
......
163 170
		case 4: //piste noire
164 171
			vitesse=30;
165 172
			vitesse_virage=30;
166
			sta_tsk(ID_capteur);
167 173
			mode=0;
168 174
			break;
169 175
		case 1: //piste verte
170
			vitesse=75;
176
			vitesse=70;
171 177
			vitesse_virage=30;
172
			mode=2;
178
			mode=0;
173 179
			break;
174 180
		case 2: //piste bleue
175
			vitesse=60;
181
			vitesse=50;
176 182
			vitesse_virage=30;
177
			mode=0;	
178
			sta_tsk(ID_capteur);	
183
			mode=0;		
179 184
			break;
180 185
		case 3: //piste rouge
181 186
			vitesse=60;
182 187
			vitesse_virage=30;
183 188
			mode=0;
184
			sta_tsk(ID_capteur);
185 189
			break;
186 190
		}
187 191
	vit_roue.data.id='V'; 
......
197 201
	CanFrame reponse;
198 202
	while(1){
199 203
		
200
    req.data.id='R'; 
201
    req.data.rtr=1;
204
			
205
    		req.data.id='R'; 
206
    		req.data.rtr=1;
202 207
 
203
    snd_dtq (CanTx,req.msg); // Interrogation du peripherique
204
    retour=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
208
    		if(s_urgence==0)snd_dtq (CanTx,req.msg); // Interrogation du peripherique
209
    		retour=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
205 210
        
206
    comm.data.id='T'; 
207
    comm.data.rtr=0; 
208
    comm.data.val=valeur;
209
    snd_dtq (CanTx,comm.msg); //on envoie la modification ? effectuer
210
	if(retour!=cons) valeur=k*(cons-retour);
211
	dly_tsk(10);
211
    		comm.data.id='T'; 
212
    		comm.data.rtr=0; 
213
    		comm.data.val=valeur;
214
    		if(s_urgence==0)snd_dtq (CanTx,comm.msg); //on envoie la modification ? effectuer
215
			if(retour!=cons) valeur=k*(cons-retour);
216
			dly_tsk(20);
217
		
212 218
	}
213 219
}
214 220

  
215 221
int calcul_virage(int a) //modifie la vitesse dans les virages
216 222
{
217 223
	if (mode==0){
218
		if (a>-30 && a<30)	return vitesse;
224
		if (a>-10 && a<10)	return vitesse;
219 225
		else return vitesse_virage;
220 226
	}
221 227
	if (mode==1){
......
245 251
			return vitesse_virage;
246 252
		}
247 253
		else return ((-(a*a)/100)+vitesse);
248
	}
249
	if (mode==3){
250
		if (a<0){
251
			return ((a*a)/200+a+vitesse);
252
		}
253
		else{ 
254
			return ((a*a)/200-a+vitesse);
255
		}
256 254
	}	
257 255
}
258 256
 	
......
261 259
	CanFrame req;
262 260
    UINT flag;
263 261
    while(1){
264
        req.data.id='U';
265
        req.data.rtr=1;
266
        periph[ADDR('U')].ev=0x02;
267
        snd_dtq(CanTx, req.msg);
262
			
263
        	req.data.id='U';
264
        	req.data.rtr=1;
265
        	periph[ADDR('U')].ev=0x02;
266
       		if(s_urgence==0)snd_dtq(CanTx, req.msg);
268 267

  
269
        wai_flg (ev_periph, 0x02, TWF_ANDW, &flag);
270
        if (periph[ADDR('U')].val>900){
271
		}
272
		else {
273
			distance=periph[ADDR('U')].val;
268
      	 	wai_flg (ev_periph, 0x02, TWF_ANDW, &flag);
269
      	 	if (periph[ADDR('U')].val>900){
274 270
			}
275
		dly_tsk(20);
271
			else {
272
				distance=periph[ADDR('U')].val;
273
				}
274
			dly_tsk(10);
275
			
276 276
	}
277 277
}
278 278

  
......
281 281
    CanFrame req,comm;
282 282
    UINT flag;
283 283
   	while (1) {
284
		
284 285
			req.data.id='D';
285 286
        	req.data.rtr=1;
286 287
        	periph[ADDR('D')].ev=0x01;
287
        	snd_dtq(CanTx, req.msg);
288
        	if(s_urgence==0)snd_dtq(CanTx, req.msg);
288 289

  
289 290
       		wai_flg (ev_periph, 0x01, TWF_ANDW, &flag);
290 291
        	ang_roue=periph[ADDR('D')].val;
......
293 294
        	comm.data.rtr=0; 
294 295
        	if(i_tremplin==1) comm.data.val=0;
295 296
			if(i_tremplin==0) comm.data.val=k2*(distance-cons_roue);		
296
		    if(i_tremplin==0 && i_ralenti==0) {
297
		    if(i_tremplin==0 && i_ralenti==0 && stop==0) {
297 298
				vit_roue.data.val=calcul_virage(k2*(distance-cons_roue)); 
298
	  			snd_dtq (CanTx,vit_roue.msg);
299
	  			if(s_urgence==0)snd_dtq (CanTx,vit_roue.msg);
299 300
        		
300 301
			}
301
			snd_dtq(CanTx, comm.msg); 
302
			dly_tsk(20);
302
			if(s_urgence==0)snd_dtq(CanTx, comm.msg); 
303
			dly_tsk(10);
303 304
		
304 305
	}
305 306
}
......
311 312
		capteur.data.id='C';
312 313
		capteur.data.rtr=1; 
313 314
		periph[ADDR('C')].ev=0x03;
314
		snd_dtq(CanTx, capteur.msg);
315
		if(s_urgence==0)snd_dtq(CanTx, capteur.msg);
315 316
		wai_flg (ev_periph, 0x03, TWF_ANDW, &flag_capt);
316 317
	
317 318
		retour_C=periph[ADDR('C')].val;
318 319
		numcapt=retour_C & 0x00ff;
320
		if ((retour_C & 0xff00)!=colcapt) {
319 321
		colcapt=retour_C & 0xff00;
322
		//AFFICHAGE DE LA COULEUR ET DU CIRCUIT
323
		switch(colcapt){
324
			case 0x5600:
325
			lcd_init();
326
			lcd_str("vert");
327
			break;
328
			case 0x4a00:
329
			lcd_init();
330
			lcd_str("jaune");
331
			break;
332
			case 0x5200:
333
			lcd_init();
334
			lcd_str("rouge");
335
			break;
336
			case 0x4200:
337
			lcd_init();
338
			lcd_str("bleu");
339
			break;
340
			case 0x4300:
341
			lcd_init();
342
			lcd_str("cyan");
343
			break;
344
		}
345
		
346
		}
347
		
348
		
320 349

  
350
//COMPTEUR DE TOUR
351
		
352
		if (retour_C==0x5605 && i_tour==0){
353
			i_tour++;
354
			compte_tour++;
355
			if (compte_tour>=3){
356
				stop=1;
357
				vitesse=0; 
358
				vitesse_virage=0;
359
				vit_roue.data.val=vitesse;
360
				snd_dtq (CanTx,vit_roue.msg);
361
			}
362
		}
363
		if (retour_C==0x5601 && i_tour!=0){
364
			i_tour=0;		
365
			}	
366
			
321 367
//CAPTEURS POUR LA PISTE ROUGE	
322 368
		if (retour_C==0x5603 && i==0 && num_piste==3){ //si on passe le capteur avant le tremplin
323 369
			i++;
324 370
			dly_tsk(1500);
325 371
			i_tremplin=1;
326
			vit_roue.data.val=45; 
327
			snd_dtq (CanTx,vit_roue.msg);
372
			vitesse=45;
373
			vitesse_virage=45;
328 374
			dly_tsk(1000);
329
			vit_roue.data.val=20; 
330
		  	snd_dtq (CanTx,vit_roue.msg);
375
			vitesse=20;
376
			vitesse_virage=20;
331 377
		}
332 378
	
333 379
		if (retour_C==0x4203 && num_piste==3){ //si on passe le capteur apr?s le tremplin
334 380
			i_tremplin=0;
335 381
			i_ralenti=1;
336
			vit_roue.data.val=20; 
337
		  	snd_dtq (CanTx,vit_roue.msg);
382
			vitesse=20;
383
			vitesse_virage=20;
338 384
			i=0;
339 385
		}
340 386
		if (retour_C==0x5604 && num_piste==3){ //on reprend les param?tres de d?part
341
			vit_roue.data.val=vitesse; 
342
		  	snd_dtq (CanTx,vit_roue.msg);
387
			vitesse=60;
388
			vitesse_virage=30;
343 389
			i_ralenti=0;
344 390
		}
345 391
		
......
359 405
	
360 406
//CAPTEURS POUR LA PISTE NOIRE
361 407
		if (retour_C==0x5601 && num_piste==4 && i==0){ //esquive des tonneaux
362
			vit_roue.data.val=vitesse; 
363
		  	snd_dtq (CanTx,vit_roue.msg);
408
			cons_roue=700;
409
			vitesse=30;
410
			vitesse_virage=30;
364 411
			i_ralenti=0;
365 412
			i_tremplin=1;
366
			dly_tsk(1500);
413
			dly_tsk(2500);
367 414
			i_tremplin=0;
368 415
			cons_roue=450;
369 416
			i++;
370 417
		}
371
		if (retour_C==0x4a01 && num_piste==4){ //ralentissement dans le virage pour se coller ? la barri?re
372
			vit_roue.data.val=10; 
373
		  	snd_dtq (CanTx,vit_roue.msg);
418
		if (retour_C==0x4a01 && num_piste==4 && i==1){ //ralentissement dans le virage pour se coller ? la barri?re
419
			vitesse=10;
420
			vitesse_virage=10;
374 421
			cons_roue=250;
375
			i_ralenti=1;
376
			i=0;
422
			i_ralenti=0;
423
			i++;
377 424
		}
378
		if (retour_C==0x5602 && num_piste==4 && i==0){ //avant la bosse de la piste noire
379
			vit_roue.data.val=10; 
380
		  	snd_dtq (CanTx,vit_roue.msg);
425
		if (retour_C==0x5602 && num_piste==4 && i==2){ //avant la bosse de la piste noire
426
			vitesse=10;
427
			vitesse_virage=10;
381 428
			cons_roue=150;
429
			dly_tsk(3500);
430
			vitesse=20;
431
			vitesse_virage=20;
432
			i++;
382 433
		}	
383 434
		if (retour_C==0x5202 && num_piste==4){ //apr?s la bosse de la piste noire
384
			vit_roue.data.val=30;
435
			vitesse=20;
436
			vitesse_virage=20;
385 437
			cons_roue=700;
386 438
			i=0;
387 439
		}
388 440
		if (retour_C==0x5603 && i==0 && num_piste==4){ //si on passe le capteur avant le tremplin
389 441
			i++;
390
			vit_roue.data.val=43; 
391
			snd_dtq (CanTx,vit_roue.msg);
442
			vitesse=43;
443
			vitesse_virage=43;
444
			cons_roue=700;
445
			dly_tsk(2000);
446
			i_tremplin=1;
392 447
			dly_tsk(1500);
393
			i_tremplin=1;
448
			i_tremplin=0;
449

  
394 450
		}
395 451
	
396
		if (retour_C==0x4203 & num_piste==4){ //si on passe le capteur apr?s le tremplin
452
		if (retour_C==0x4203 && num_piste==4 && i==1){ //si on passe le capteur apr?s le tremplin
397 453
			i_tremplin=0;
398
			i_ralenti=1;
399
			vit_roue.data.val=20; 
400
		  	snd_dtq (CanTx,vit_roue.msg);
401
			i=0;
454
			i_ralenti=0;
455
			vitesse=5;
456
			vitesse_virage=5;
457
			i++;
458
			dly_tsk(2500);
459
			vitesse=20;
460
			vitesse_virage=20;
461
			cons_roue=450;
402 462
		}
403 463
		
404
		if (retour_C==0x5604 & num_piste==4){ //on se d?cale et on ralentit pour les graviers
405
			cons_roue=450;
406
			vit_roue.data.val=15; 
407
		  	snd_dtq (CanTx,vit_roue.msg);
408
			i_ralenti=1;
464
		if (retour_C==0x5604 && num_piste==4){ //on se d?cale et on ralentit pour les graviers
465
			cons_roue=300;
466
			vitesse=15;
467
			vitesse_virage=15;
468
			i_ralenti=0;
409 469
		}
410
		if (retour_C==0x4304 & num_piste==4){ //on reprend les param?tres de d?part
470
		if (retour_C==0x4304 && num_piste==4 && i!=0){ //on reprend les param?tres de d?part
471
			cons_roue=500;
472
			vitesse=20;
473
			vitesse_virage=20;
474
			i=0;
475
			dly_tsk(1500);
411 476
			cons_roue=700;
412
			vit_roue.data.val=20; 
413
		  	snd_dtq (CanTx,vit_roue.msg);
414 477
		}
415 478
		
416
		dly_tsk(20);
479
	dly_tsk(20);}
480
}
481

  
482
void urgence()
483
{
484
	while(1){
417 485
		
486
		while (p0_4==0){
487
				s_urgence=1;
488
				vit_roue.data.val=0; 
489
				snd_dtq (CanTx,vit_roue.msg);
490
				cu=1;
491
				i_urgence=0;
492
			}
493
		if (p0_4==1 && i_urgence==0){
494
				i_urgence==1;
495
				s_urgence=0;
496
				vit_roue.data.val=vitesse; 
497
		  		snd_dtq (CanTx,vit_roue.msg);
498
				cu=1;		
499
			}
500
	dly_tsk(20);
418 501
	}
419 502
}
420

  
421 503

  
422 504
void main()
423 505
{	
......
436 518
	sta_tsk(ID_mes_dist);
437 519
	sta_tsk(ID_angle_roue);
438 520
	sta_tsk(ID_demarre);
439
	//sta_tsk(ID_capteur);
521
	sta_tsk(ID_capteur);
522
	sta_tsk(ID_urgence);
440 523
	
441
    while(1){/*
442
*/
524
    while(1){
525
	/*			
526
		while (p0_4==0){
527
				s_urgence=1;
528
				vit_roue.data.val=0; 
529
				snd_dtq (CanTx,vit_roue.msg);
530
				i_urgence=0;
531
			}
532
			if (p0_4==1 && i_urgence==0){
533
				i_urgence==1;
534
				s_urgence=0;
535
			}*/
443 536
	}	 
444 537
}
445 538

  

Also available in: Unified diff