Revision 238 branch/vilain_sebastien/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
124 124
void commande_angle_roues(short angle_roues);
125 125
short lecture_angle();
126 126
short lecture_telemetre();
127
short lecture_capteur();
127 128
void commande_vitesse(short vitesse);
128
void choix_circuit(int i);
129
void init_vitesse(short vitesse);
129 130
short consigne_angle = 450;
130
short erreur;
131
short vitesse = 120;
132
short consigne_telemetre = 714;
131
short consigne_telemetre = 705;
133 132
FLGPTN flag;
134 133
short dist;
134
short angle_roues;
135
int rampe = 0;
136
short lecture;
135 137

  
136 138
void main()
137 139
{
......
139 141
	ports_mcu();
140 142
    lcd_init();
141 143
	periph_init();
142
	periph_nom("#Test*");
144
	periph_nom("#Seb*");
143 145

  
144 146
    can_init();
145 147
    clavier_init(1);
146 148
	capture_init();
147 149
	
148 150
	periph[ADDR('R')].ev=0x01;
151
	periph[ADDR('U')].ev=0x02;
152
	periph[ADDR('C')].ev=0x03;
149 153
	
150 154
	sta_cyc(ID_acqui);
151 155
	sta_tsk(ID_periph_rx);
152 156
	sta_tsk(ID_asserv0);
153 157
	sta_tsk(ID_asserv1);
158
	sta_tsk(ID_commande_capteur);
154 159

  
155
	commande_vitesse(35);
160
	init_vitesse(45);
156 161
	
157 162
    while(1)
158 163
    {
......
180 185

  
181 186
short lecture_angle()
182 187
{
183
	CanFrame requete, reponse;
188
	CanFrame requete;
184 189
	
185 190
	requete.data.id = 'R';
186 191
	requete.data.rtr = 1;
......
198 203
	{
199 204
		erreur = K*(consigne_angle-lecture_angle());
200 205
		commande_tourelle(erreur);
201
		dly_tsk(98);
206
		dly_tsk(25);
202 207
	}
203 208
}
204 209

  
......
214 219

  
215 220
short lecture_telemetre()
216 221
{
217
	CanFrame requete, reponse;
222
	CanFrame requete;
218 223
	
219 224
	requete.data.id = 'U';
220 225
	requete.data.rtr = 1;
221 226
	snd_dtq(CanTx, requete.msg);
222
	wai_flg (ev_periph,0x01,TWF_ORW,&flag);
223
	clr_flg(ev_periph,~0x01);
227
	wai_flg (ev_periph,0x02,TWF_ORW,&flag);
228
	clr_flg(ev_periph,~0x02);
224 229
	return (periph[ADDR('U')].val);
225 230
}
226 231

  
227 232
void asserv1()
228 233
{
229
	int K = 1;
234
	int K = 2;
230 235
	short erreur;
231 236
	short ancien_dist;
232 237
	while(1)
233 238
	{
234
		ancien_dist = dist;
235
		dist = lecture_telemetre();
236
		if(dist >= 1000)
239
		if (rampe == 0)
237 240
		{
238
			dist = ancien_dist;
241
			ancien_dist = dist;
242
			dist = lecture_telemetre();
243
			if((dist >= 2000)||(dist <= -2000))
244
			{
245
				dist = ancien_dist;
246
			}
247
			erreur = K*(consigne_telemetre-dist);
248
			angle_roues = -erreur;
249
			commande_angle_roues(-erreur);
239 250
		}
240
		erreur = K*(consigne_telemetre-dist);
241
		commande_angle_roues(-erreur);
242
		dly_tsk(98);
251
		dly_tsk(5);
243 252
	}
244 253
}
245 254

  
246 255
void commande_vitesse(short vitesse)
256
{	
257
	CanFrame comm;
258
	
259
	comm.data.id = 'V';
260
	comm.data.rtr = 0;
261
	comm.data.val=vitesse;
262
	snd_dtq(CanTx, comm.msg);
263
}
264

  
265
void init_vitesse(short vitesse)
247 266
{
267
	dly_tsk(3000);
268
	commande_vitesse(vitesse);
269
}
270

  
271
short lecture_capteur()
272
{
248 273
	CanFrame requete;
249 274
	
250
	dly_tsk(3000);
251
	requete.data.id = 'V';
252
	requete.data.rtr = 0;
253
	requete.data.val=vitesse;
275
	requete.data.id = 'C';
276
	requete.data.rtr = 1;
254 277
	snd_dtq(CanTx, requete.msg);
278
	wai_flg (ev_periph,0x03,TWF_ORW,&flag);
279
	clr_flg(ev_periph,~0x03);
280
	return (periph[ADDR('C')].val);
281
}
282

  
283
void commande_capteur()
284
{
285
	while(1)
286
	{
287
		lecture = lecture_capteur();
288
		if((lecture & 0xFF00) == 0x6200) //bleu
289
		{
290
			rampe = 1;
291
		}
292
		if ((lecture & 0xFF00) == 0x7600) //vert
293
		{
294
			rampe = 0;
295
			commande_vitesse(20);
296
		}
297
		if ((lecture & 0xFF00) == 0x6a00) //jaune
298
		{
299
			rampe = 0;
300
		}
301
		if ((lecture & 0xFF00) == 0x5604)
302
		{
303
			commande_vitesse(45);
304
		}
305
		if ((lecture & 0xFF00) == 0x7200) //rouge
306
		{
307
			consigne_telemetre = 600;
308
		}
309
		if ((lecture & 0xFF00) == 0x6300) //cyan
310
		{
311
			consigne_telemetre = 705;
312
		}
313
		dly_tsk(100);
314
	}
255 315
}

Also available in: Unified diff