Revision 372 branch/porte/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
118 118
//'j'/106/06A : R?cup?ration du r?sultat de dernier code envoy?. 0x77 si aucun code n'a ?t? soumis. <0 si la r?ponse n'est pas
119 119
//				disponible. 0xab avec a-> nombre de couleurs bien plac?es et b -> couleurs pr?sentes mais mal plac?es.
120 120
//'I'/73/Ox49 : D?finition du nom du v?hicule. Doit d?buter par le caract?re '#' et entraine le chargement de la configuration de piste
121
//				correspondant au nom du v?hicule si le nom se termine par '*'
121
//				correspondant au nom du v?hicule si le nom se termine par '*'
122

  
123
// +-----------------------------------------------------+
124
// |     				Constantes           		     |
125
// +-----------------------------------------------------+
126
const Vitesse_max=99, Vitesse_min=0, Vitesse_noir=12;
127
const Kpt=1 , Kdt=0.1, Kdr=0.2, Kpr=1;											//Param?tre des PD de la tourelle et des roues
122 128

  
123
/*-------------------------------------- Constante ----------------------------------------*/
124
const Vitesse_max=99, Vitesse_min=0;
125

  
126
/*-------------------------------------- Variables ----------------------------------------*/
129
// +-----------------------------------------------------+
130
// |     				Variables           		     |
131
// +-----------------------------------------------------+
127 132
//Variables pour l'asserivssement des roues, de la vitesse et du telemetre
128
unsigned int Angle_T=0, Consigne_T=500;						//prend la valeur de l'angle au temps t-1 //Consigne de l'angle de la tourelle (consigne/10=>degr?s)
133
unsigned int Angle_T=0, Consigne_T=400, Consigne_T_noir=900;						//prend la valeur de l'angle au temps t-1 //Consigne de l'angle de la tourelle (consigne/10=>degr?s)
129 134
unsigned int valeur=0, Saut=0; 										//valeur transmise corrig?
130 135
unsigned int Somme_Erreur=0, Erreur=0;			
131 136
unsigned int ErreurRoues=0,Somme_ErreurRoues=0;
132
int Kpt=1,	Kpr=1;											//Param?tre des PID tourelle et roues
133
int Kdt=0.1,	Kdr=0.1;
134
unsigned int Vitesse=40, vit=0, i=1, Distance;		
135
unsigned int Angle_R=0, distance_bord=660;					//Angle des roues
137
unsigned int vit=0, i=1, Distance;		
138
unsigned int Angle_R=0, Vitesse_vert=40,Vitesse_bleu=35,Vitesse_rouge=30;					//Angle des roues
136 139
//Variable de la fonction Capteur
137
unsigned int Info_capteur;
140
unsigned int Info_capteur, couleur_capteur, num_capteur;
138 141
//Variable de la fonction Distance_bord
139
unsigned int Bord_defaut=0, ecart=400;
142
unsigned int Bord_defaut=0, ecart=400, distance_bord=790, distance_bord_noir=500;
140 143
//Variable des fonctions IHM
141
short Touche_clavier, Temps;
144
short Touche_clavier, Temps=0;
142 145
//char *Temps;
143 146
//Variable de la fonction Circuit
144
unsigned int Info_circuit, Etat_feu, Num_circuit, Depart_ok=0, Arret_urgence=0;
147
unsigned int Info_circuit, Etat_feu, Num_circuit=0, Depart_ok=0, Arret_urgence=0;
145 148
//Variable de la fonction Evenement
146 149
unsigned int Lecture_eve;
147 150
FLGPTN test;
148 151

  
149

  
152

  
153
// +-----------------------------------------------------+
154
// |     			Asservissement tourelle           	 |
155
// +-----------------------------------------------------+
150 156
void Asserv_T(){	
151 157
	
152 158
	//'T'/84/0x54?: Commande en vitesse de la tourelle portant le telemetre (en 1/10 de degre/secondes).
......
158 164
			
159 165
		demande.data.id='R'; 
160 166
		demande.data.rtr=1;
167
		
161 168
	
162
		snd_dtq (CanTx,demande.msg); // Interrogation du peripherique
163
		Angle_T=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
169
		snd_dtq (CanTx,demande.msg); 			// Interrogation du peripherique sur la position de la tourelle
170
		Angle_T=periph[ADDR('R')].val; 			// contient la valeur de retour du simulateur de l'angle de la tourelle.
164 171
		
165 172
		comm.data.id='T'; 
166 173
		comm.data.rtr=0; 
167
		comm.data.val=valeur;
168
		snd_dtq (CanTx,comm.msg);//Envoi de la commande
174
		comm.data.val=valeur;					// Commande de l'angle de la tourelle 
175
		snd_dtq (CanTx,comm.msg);				// Envoi de la commande		
169 176
		
170 177
		dly_tsk(100);
171 178
		}
......
173 180

  
174 181
void Asserv_T_hc(){
175 182
	
176
	Erreur=Consigne_T-Angle_T;
177
	Somme_Erreur+=Erreur;
178
	
183
	switch(Num_circuit){			
184
			case 1:
185
				Erreur=Consigne_T-Angle_T;
186
			break;
187
			case 2:
188
				Erreur=Consigne_T-Angle_T;
189
			break;
190
			case 3:
191
				Erreur=Consigne_T-Angle_T;
192
			break;
193
			case 4:
194
				 Erreur=Consigne_T_noir-Angle_T;
195
			break;			
196
		}
179 197
	if(Angle_T!=Consigne_T)
180
		valeur=Kpt*Erreur+Kdt*Somme_Erreur;//correction de l'angle par handler cyclique de la tourelle
198
		valeur=Kpt*Erreur+Kdt*Somme_Erreur;					//correction de l'angle par handler cyclique de la tourelle
181 199
	}
182 200

  
201
// +-----------------------------------------------------+
202
// |     			Asservissement Roues	           	 |
203
// +-----------------------------------------------------+
204

  
183 205
void Asserv_R(){
184 206
	
185 207
	//'D'/68/0x44?: Commande de l'angle des roues directrices (en 1/10 de degre).
......
187 209
	CanFrame demande;
188 210
	CanFrame reponse;
189 211
	
190
	dly_tsk(1500);
212
	dly_tsk(1000);
191 213
	while(1){
192 214
		
193 215
		comm.data.id='D'; //Commande de l'angle des roues
......
203 225
	
204 226
	ErreurRoues=distance_bord-Distance;
205 227
	Somme_ErreurRoues+=ErreurRoues;
206
	
207
	if(Distance != distance_bord && Bord_defaut == 0)
208
		Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);//correction de l'angle par handler cyclique des roues
228
	switch(Num_circuit){
229
			case 1:
230
				if(Distance != distance_bord && Bord_defaut == 0){
231
					Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}				//correction de l'angle par handler cyclique des roues
232
			break;
233
			case 2:
234
				if(Distance != distance_bord && Bord_defaut == 0){
235
					Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}				//correction de l'angle par handler cyclique des roues
236
			break;
237
			case 3:
238
				if(Distance != distance_bord && Bord_defaut == 0 && num_capteur != 0x66F0){
239
					Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}				//correction de l'angle par handler cyclique des roues					
240
			break;
241
			case 4:
242
				if(Distance != distance_bord_noir && Bord_defaut == 0){
243
					Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}				//correction de l'angle par handler cyclique des roues
244
			break;
245
			default : 
246
				if(Distance != distance_bord && Bord_defaut == 0){
247
				Angle_R=-1*(Kpr*ErreurRoues+Kdr*Somme_ErreurRoues);}
209 248
	}
249
}
210 250

  
211

  
251
// +-----------------------------------------------------+
252
// |     			Asservissement Vitesse           	 |
253
// +-----------------------------------------------------+
212 254
void Asserv_V(){
213 255
	
214 256
	//'V'/86/0x56?: Commande en vitesse des roues motrices du vehicule (en radian /secondes).
......
216 258
	CanFrame demande;
217 259
	CanFrame reponse;
218 260
	
219
	dly_tsk(3000);
261
	dly_tsk(2500);
220 262
	while(1){		
221 263
	
222 264
		comm.data.id='V'; 						//Commande de la vitesse des roues motrices
223 265
		comm.data.rtr=0;
224
	
225
		if(Depart_ok == 1 && Arret_urgence == 0){		
226
			if(Info_capteur == 0x560){				// Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
227
				comm.data.val=Vitesse;	
228
				snd_dtq (CanTx,comm.msg);			// Envoi de la commande de vitesse
229
			}
230
			else if(Info_capteur == 0x630 && Saut == 0){		//Accel?ration pour passer le saut
231
				comm.data.val=Vitesse*1.66;	
266
		
267
		switch(Num_circuit){			
268
			case 1:	
269
				if(Depart_ok == 1 && Arret_urgence == 0){		
270
					if(couleur_capteur == 0x560){				// Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
271
						comm.data.val=Vitesse_vert;	
272
						snd_dtq (CanTx,comm.msg);			// Envoi de la commande de vitesse
273
					}
274
					else{									// Ralentissement en cas de virage.
275
						comm.data.val=Vitesse_vert/1.2;	
276
						snd_dtq (CanTx,comm.msg);
277
					}
278
				}
279
				else{
280
					comm.data.val=0;	
281
					snd_dtq (CanTx,comm.msg);
282
				}
283
			break;
284
			case 2:	
285
				if(Depart_ok == 1 && Arret_urgence == 0){		
286
					if(couleur_capteur == 0x560){				// Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
287
						comm.data.val=Vitesse_bleu;	
288
						snd_dtq (CanTx,comm.msg);			// Envoi de la commande de vitesse
289
					}
290
					else{									// Ralentissement en cas de virage.
291
						comm.data.val=Vitesse_bleu/1.3;	
292
						snd_dtq (CanTx,comm.msg);
293
					}
294
				}
295
				else{
296
					comm.data.val=0;	
297
					snd_dtq (CanTx,comm.msg);
298
				}
299
			break;
300
			case 3 :
301
				if(Depart_ok == 1 && Arret_urgence == 0){
302
					if(couleur_capteur == 0x560){				// Commande de vitesse pour les lignes droites (dernier capteur detecte = vert).
303
					comm.data.val=Vitesse_rouge;	
304
					snd_dtq (CanTx,comm.msg);			// Envoi de la commande de vitesse
305
					}
306
					else if(couleur_capteur == 0x0766 && num_capteur == 0x66F0){				//Accel?ration pour passer le saut
307
					comm.data.val=45;	
308
					snd_dtq (CanTx,comm.msg);
309
					}
310
					else{									// Ralentissement en cas de virage.
311
					comm.data.val=Vitesse_rouge/1.5;	
312
					snd_dtq (CanTx,comm.msg);
313
					}
314
				}
315
			break;
316
			case 4 : 
317
				if(Depart_ok == 1 && Arret_urgence == 0){
318
					comm.data.val=Vitesse_noir;	
319
					snd_dtq (CanTx,comm.msg);
320
				}
321
			break;					
322
			default : 
323
				comm.data.val=0;	
232 324
				snd_dtq (CanTx,comm.msg);
233
				Saut=1;
234
			}
235
			else if(Info_capteur == 0x630 && Saut == 1){		//Ralentissement pour la reception du saut
236
				comm.data.val=Vitesse/2.3;	
237
				snd_dtq (CanTx,comm.msg);
238
			//	Saut=0;
239
			}
240
			else{									// Ralentissement en cas de virage et de d?tection de trou ou d'obstacle.
241
				comm.data.val=Vitesse/1.55;	
242
				snd_dtq (CanTx,comm.msg);
243
			}
244 325
		}
245
		else{
246
			comm.data.val=0;	
247
			snd_dtq (CanTx,comm.msg);
248
		}
249 326
				
250 327
	dly_tsk(10);
251 328
	}
252 329
}
253 330

  
331
// +-----------------------------------------------------+
332
// |     			Commande capteur		           	 |
333
// +-----------------------------------------------------+
254 334
void Capteur(){
255 335
	
256 336
	//'C'/67/0x43?: Informations sur le dernier capteur touche :
......
264 344
		
265 345
		demande.data.id='C';
266 346
		demande.data.rtr=1;
267
		snd_dtq (CanTx,demande.msg); 			// Interrogation du peripherique sur les donnees du dernier capteur touch?.
268
		Info_capteur=periph[ADDR('C')].val>>4; 		// contient la valeur de retour du simulateur sur le dernier capteur touch?.
347
		snd_dtq (CanTx,demande.msg); 				// Interrogation du peripherique sur les donnees du dernier capteur touch?.
348
		Info_capteur=periph[ADDR('C')].val; 		// contient la valeur de retour du simulateur sur le dernier capteur touch?.
349
		couleur_capteur=Info_capteur >> 4;
350
		num_capteur=Info_capteur << 4;
269 351
		
270 352
		dly_tsk(100);
271 353
	}
......
273 355
	
274 356
}
275 357

  
358

  
359
// +-----------------------------------------------------+
360
// |     			Gestion des troues		           	 |
361
// +-----------------------------------------------------+
362

  
276 363
void Distance_bord(){
277 364
	
278 365
	//'U'/85/0x55?: Distance mesuree par le telemetre (1/100 de metre).
......
288 375
		snd_dtq (CanTx,demande.msg); // Interrogation du peripherique sur la distance mesure par le telemetre.
289 376
		Distance=periph[ADDR('U')].val; // contient la valeur de retour du simulateur sur la distance mesur? par le telemetre.
290 377
		
291
		if(Distance > distance_bord+ecart  || Distance < distance_bord-ecart){
292
			Bord_defaut=1;
378
		switch(Num_circuit){
379
			case 1:
380
				if(Distance > distance_bord+ecart  || Distance < distance_bord-ecart){
381
				Bord_defaut=1;
382
				}
383
				else{
384
				Bord_defaut=0;
385
				}
386
			break;
387
			case 2:
388
				if(Distance > distance_bord+ecart  || Distance < distance_bord-ecart){
389
				Bord_defaut=1;
390
				}
391
				else{
392
				Bord_defaut=0;
393
				}
394
			break;
395
			case 3:
396
				if(Distance > distance_bord+ecart  || Distance < distance_bord-ecart){
397
				Bord_defaut=1;
398
				}
399
				else{
400
				Bord_defaut=0;
401
				}
402
			break;
403
			case 4:
404
				if(Distance > distance_bord_noir+ecart  || Distance < distance_bord_noir-ecart){
405
				Bord_defaut=1;
406
				}
407
				else{
408
				Bord_defaut=0;
409
				}
410
			break;
293 411
		}
294
		else{
295
			Bord_defaut=0;
296
		}		
297
		dly_tsk(10);
412
		dly_tsk(10);	
298 413
	}
299
	
300 414
}
301 415

  
416
// +-----------------------------------------------------+
417
// |     			Commande IHM			           	 |
418
// +-----------------------------------------------------+
302 419
void Commande_clavier(){
303 420
	
304 421
	// Interface Homme Machine muni du clavier matricielle, de l'?cran LCD,
......
315 432
			}
316 433
			else if(Touche_clavier == 42 || Touche_clavier == 35){
317 434
				if(vit < 100 && vit > 0){
318
					Vitesse = vit;
435
					Vitesse_vert = vit;
319 436
				}
320 437
				vit = 0;
321 438
				i = 1;			
......
375 492
	
376 493
}
377 494

  
495
// +-----------------------------------------------------+
496
// |     Gestion des informations de la piste		   	 |
497
// +-----------------------------------------------------+
378 498
void Circuit(){
379 499
	
380 500
	//'M'/77/0x7D?: Mode de course :
......
415 535
	}
416 536
		
417 537
}
418

  
538

  
539
// +-----------------------------------------------------+
540
// |  					   main		  				 	 |
541
// +-----------------------------------------------------+
419 542
void main()
420 543
{
421 544
	ports_mcu();

Also available in: Unified diff