Revision 257 branch/belin/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
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 121
//				correspondant au nom du v?hicule si le nom se termine par '*'
122 122

  
123

  
124
unsigned short Distance;
125 123
unsigned short pos_tourelle;
124
unsigned short capt=0;
125
unsigned short GEL=0;
126 126

  
127

  
128 127
	
129 128

  
130 129
void asservissementTourelle(){
......
148 147
	retour=periph[ADDR('R')].val; 
149 148
	
150 149
	//Asservissement
151
	correction=5*(consigne-retour);
150
	correction=4*(consigne-retour);
152 151
	comm.data.id='T';
153 152
	comm.data.rtr = 0;
154 153
	comm.data.val = correction;
155 154
	
156 155
	snd_dtq(CanTx,comm.msg);
157
	dly_tsk(100);
156
	dly_tsk(10);
158 157
	}
159 158

  
160 159
}
......
168 167
	snd_dtq(CanTx,commMoteur.msg);
169 168
}
170 169

  
171
void CalculDistance(){
170
void position(){
172 171
	CanFrame req;
173 172
	UINT flag;
174

  
173
	CanFrame comm;
174
	unsigned short correction,Distance,consigne;
175
	consigne=700;
176
	
175 177
	while(1){
176

  
178
			req.data.id = 'U';
179
			req.data.rtr = 1;
180
			snd_dtq(CanTx, req.msg);
181
			Distance=periph[ADDR('U')].val; 
182
		if(Distance>1500 || GEL==1){
183
				comm.data.id='D';
184
				comm.data.rtr = 0;
185
				comm.data.val = 0;
186
				dly_tsk(10);
187
		}
188
		else{
189
				//Lecture de distance	
190
				req.data.id = 'U';
191
				req.data.rtr = 1;
192
				snd_dtq(CanTx, req.msg);
193
				Distance=periph[ADDR('U')].val; 
194
				//Asservissement
195
				correction=1*(Distance-consigne);
196
				comm.data.id='D';
197
				comm.data.rtr = 0;
198
				comm.data.val = correction;
177 199
	
178
	req.data.id = 'U';
179
	req.data.rtr = 1;
180
	periph[ADDR('R')].ev=0x01;
181
	snd_dtq(CanTx, req.msg);
182
	wai_flg(ev_periph,0x01,TWF_ANDW, &flag);
183

  
184
	Distance=periph[ADDR('U')].val; 
185
	dly_tsk(100);
186
	}
200
				snd_dtq(CanTx,comm.msg);
201
				dly_tsk(10);
202
		}
203
			dly_tsk(10);
204
	}	
187 205
}
188 206

  
189
void position(){
190
	unsigned short correction;
207
void capteur(){
208
	CanFrame req;
209
	UINT flag;
191 210
	CanFrame comm;
192
	while(1){
193
	correction=1*(720-Distance);
194
	comm.data.id='D';
195
	comm.data.rtr = 0;
196
	comm.data.val = correction;
197
	snd_dtq(CanTx,comm.msg);
198
	dly_tsk(100);
199
	LED_R=1;
211
	while (1){
212
			req.data.id = 'C';
213
			req.data.rtr = 1;
214
			snd_dtq(CanTx, req.msg);
215
			capt=periph[ADDR('C')].val;
216
			GEL=0;
217
		while(capt==0x7610){
218
			GEL=1;
219
			req.data.id = 'C';
220
			req.data.rtr = 1;
221
			snd_dtq(CanTx, req.msg);
222
			capt=periph[ADDR('C')].val;
223
			commMoteur(45);
224
			comm.data.id='D';
225
			comm.data.rtr = 0;
226
			comm.data.val = 0;
227
			snd_dtq(CanTx,comm.msg);
228
			dly_tsk(100);
229
		}
230
	 dly_tsk(10);
200 231
	}
201 232
}
202 233
		
203
	
234
		
235
			
204 236
void main()
205 237
{
206 238
	ports_mcu();
207 239
    lcd_init();
208 240
	periph_init();
209
	periph_nom("Matthias");
241
	periph_nom("#BelinMob*");
210 242
	can_init();
211 243
    clavier_init(1);
212 244
	capture_init();
......
214 246

  
215 247

  
216 248
	sta_tsk(ID_periph_rx);
217
	commMoteur(5);
218 249
	sta_tsk(ID_tourelle);
219
	sta_tsk(ID_CalculDistance);
220 250
	sta_tsk(ID_position);
221

  
251
	sta_tsk(ID_capteur);
252
	dly_tsk(1500);
253
	commMoteur(30);
222 254
	
223 255
    while(1)
224 256
    {

Also available in: Unified diff