Revision 62 branch/vilain_sebastien/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
63 63
// 		comm.data.id='R'; comm.data.rtr=1;
64 64
// 		snd_dtq (CanTx,comm.msg);
65 65
//
66
// Des l'arrivee de la reponse du simlateur, les variables suivantes sont mises a jour:
66
// Des l'arrivee de la reponse du simulateur, les variables suivantes sont mises a jour:
67 67
// periph[ADDR('R')].val : contient la derniere valeur renvoyee par le simulateur.
68 68
//
69 69
// Pour verifier si une nouvelle valeur a ete recue utiliser:
......
121 121
//				correspondant au nom du v?hicule si le nom se termine par '*'
122 122

  
123 123
void commande_tourelle(short vit_rot);
124
void commande_angle_roues(short angle_roues);
124 125
short lecture_angle();
125
short asserv0(short consigne);
126
short consigne = 900;
126
short lecture_telemetre();
127
void commande_vitesse(short vitesse);
128
short consigne_angle = 450;
127 129
short erreur;
128 130
short vitesse = 120;
131
short consigne_telemetre = 750;
132
FLGPTN flag;
133
short dist;
129 134

  
130 135
void main()
131 136
{
......
138 143
    can_init();
139 144
    clavier_init(1);
140 145
	capture_init();
141

  
146
	
147
	periph[ADDR('R')].ev=0x01;
148
	
142 149
	sta_cyc(ID_acqui);
143
//	sta_tsk(ID_periph_rx);
150
	sta_tsk(ID_periph_rx);
151
	sta_tsk(ID_asserv0);
152
	sta_tsk(ID_asserv1);
144 153

  
145
	
146
	requete.data.id = 'V';
147
	requete.data.rtr = 0;
148
	requete.data.val=30;
149
	snd_dtq(CanTx, requete.msg);
154
	commande_vitesse(40);
155
	
150 156
    while(1)
151 157
    {
152
		erreur = asserv0(consigne);
153
		commande_tourelle(erreur);
158
		dist=lecture_telemetre();
154 159
		LED_J=1;
155 160
		dly_tsk(100);
156 161
		LED_J=0;
......
180 185
	requete.data.id = 'R';
181 186
	requete.data.rtr = 1;
182 187
	snd_dtq(CanTx, requete.msg);
183
	
184
	rcv_dtq(CanRx, &reponse.msg);
185
	return (reponse.data.val);
188
	wai_flg (ev_periph,0x01,TWF_ORW,&flag);
189
	clr_flg(ev_periph,~0x01);
190
	return (periph[ADDR('R')].val);
186 191
}
187 192

  
188
short asserv0(short consigne)
193
void asserv0()
189 194
{
190 195
	int K = 2;
191 196
	short erreur;
192
	erreur = K*(consigne-lecture_angle());
193
	return erreur;
197
	while(1)
198
	{
199
		erreur = K*(consigne_angle-lecture_angle());
200
		commande_tourelle(erreur);
201
		dly_tsk(98);
202
	}
203
}
204

  
205
void commande_angle_roues(short angle_roues)
206
{
207
	CanFrame comm;
208
	
209
	comm.data.id = 'D';
210
	comm.data.rtr = 0;
211
	comm.data.val = angle_roues;
212
	snd_dtq(CanTx, comm.msg);
213
}
214

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

  
227
void asserv1()
228
{
229
	int K = 1;
230
	short erreur;
231
	while(1)
232
	{
233
		erreur = K*(consigne_telemetre-lecture_telemetre());
234
		commande_angle_roues(-erreur);
235
		dly_tsk(98);
236
	}
237
}
238

  
239
void commande_vitesse(short vitesse)
240
{
241
	CanFrame requete;
242
	
243
	dly_tsk(3000);
244
	requete.data.id = 'V';
245
	requete.data.rtr = 0;
246
	requete.data.val=vitesse;
247
	snd_dtq(CanTx, requete.msg);
194 248
}

Also available in: Unified diff