Revision 284 branch/guochao/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 '*'
122

  
123

  
121
//				correspondant au nom du v?hicule si le nom se termine par '*'
122
#include "pid.h"
123

  
124
CanFrame comm;
125
CanFrame requete;
126
CanFrame reponse;
127

  
128
int g_angle450=450;
129
int position = 0;
130
int erreur = 0;
131

  
132
void corrigerAngle()
133
{
134
	int angle;
135
	int erreur;
136
	
137
	requete.data.rtr = 1;
138
	comm.data.rtr = 0;
139
	while(1)
140
	{
141
		// lecture
142
		requete.data.id  = 'R';
143
		requete.data.rtr = 1;
144
		snd_dtq(CanTx, requete.msg);
145
		dly_tsk(5);
146
		angle = periph[ADDR('R')].val;//reponse.data.val;
147
	
148
			// corriger
149
		erreur = g_angle450 - angle;//** (ok)
150
		comm.data.id = 'T';
151
		comm.data.val = erreur;
152
		snd_dtq(CanTx, comm.msg);
153
		dly_tsk(20);	
154
	}
155
}
156

  
157
void setVitesse(int vitesse)
158
{//**
159
	comm.data.id = 'V';
160
	comm.data.rtr = 0;
161
	comm.data.val = vitesse;
162
	snd_dtq(CanTx, comm.msg);	
163
}
164

  
165
void mesurerDistance()
166
{
167
	requete.data.rtr = 1;
168
	
169
	while(1)
170
	{
171
		requete.data.id  = 'U';
172
		snd_dtq(CanTx, requete.msg);
173
		dly_tsk(5);
174
		position = periph[ADDR('U')].val;
175
		dly_tsk(20);
176
	}
177
}
178

  
179
void controleRoue(int val)
180
{//**
181
	comm.data.id = 'D';
182
	comm.data.rtr = 0;
183
	comm.data.val = val;
184
	snd_dtq(CanTx, comm.msg);	
185
}
186

  
187
void afficher(int val)
188
{//** while ...
189
	char str[20];
190
	
191
	lcd_cls();
192
	sprintf(str, "Distance: %d", val);
193
	lcd_str(str);	
194
}
195

  
196
int getStatue(int erreur)
197
{
198
	if( abs(erreur) > 30 ) //abs
199
		return 1;
200
	else
201
		return 0;
202
}
203

  
204
char coleur;
205
char num;
206
short capteur;
207
int valider;
208
int siSauter(void)
209
{//** Tache ...
210
CanFrame requete;
211

  
212
	requete.data.id  = 'C';
213
	requete.data.rtr = 1;
214
	snd_dtq(CanTx, requete.msg);
215
	dly_tsk(5);	
216
	capteur = periph[ADDR('C')].val;
217
	coleur = capteur >> 8;
218
	num    = capteur & 0xff;
219

  
220
	if( 15 == num && coleur == 'r' && valider)
221
		return 1;
222
	else
223
		return 0;	
224
}
225

  
226
int sauter = 0;
227
int statue;
124 228
void main()
125
{
229
{
230
	float count = 0;
231
	int lastErreur = 0;
232
	valider = 1;
233
	
126 234
	ports_mcu();
127 235
    lcd_init();
128 236
	periph_init();
......
131 239
    can_init();
132 240
    clavier_init(1);
133 241
	capture_init();
242
	IncPIDInit(POSITION_STANDAR_45, 0);
134 243

  
135
	sta_cyc(ID_acqui);
136
//	sta_tsk(ID_periph_rx);
244
	sta_cyc(ID_acqui);
245
//	sta_cyc(ID_itouche);
246
	sta_tsk(ID_clavier);
247
	sta_tsk(ID_periph_rx);
248
	sta_tsk(ID_corrigerAngle);
249
	sta_tsk(ID_mesurerDistance);
250
	
137 251

  
138 252
    while(1)
139
    {
140
		LED_J=1;
141
		dly_tsk(100);
142
		LED_J=0;
143
		dly_tsk(100);
253
    {//*** Tache 
254
		erreur = IncPIDCalc(position);
255
		if( erreur > 200 || erreur < -200 )			// evider le tronc
256
			erreur = lastErreur;
257
		
258
	//	statue = getStatue(erreur);
259
		
260
		sauter = siSauter();
261
	
262
		if( (1 == sauter) && (count < 150) )
263
		{
264
			setVitesse(45);
265
			erreur = lastErreur;
266
			count++;
267
		}
268
		else if( count*1.5 > 0 )
269
		{
270
			count -= 0.5;
271
			valider = 0;
272
			setVitesse(0);
273
			erreur = lastErreur;
274
		}
275
		else
276
		{
277
			setVitesse(30);
278
			count = 0;
279
			valider = 1;
280
		}
281
		controleRoue(erreur);
282
		lastErreur = erreur;
144 283
    }
145 284
}
285
/*
286
void itouche()
287
{
288
	char t;
289
	
290
	if(t=clavier_scan())
291
		vipsnd_dta(QdmTouche,t);
292
}
293
*/
294
void clavier()
295
{
296
	short t;
297
	int vitesse=20;
298
	
299
	while(1)
300
	{
301
		vrcv_dtq(QdmTouche, &t);
302
		
303
		if(t == '2')
304
		{
305
			vitesse += 2;
306
			setVitesse(vitesse);
307
		}
308
		if(t == '8')
309
		{
310
			vitesse -= 2;
311
			setVitesse(vitesse);
312
		}
313
		
314
	}
315
}
146 316

  
147 317
void acqui()
148 318
{

Also available in: Unified diff