Revision 349 branch/fichot/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
121 121
//				correspondant au nom du v?hicule si le nom se termine par '*'
122 122

  
123 123

  
124
int k=5;
125
int k2=1;
126
int vitesse = 30;
124
int k1=4;
125
int k2=2;
126
UINT flag;
127
unsigned short capt=0;
128
unsigned short stop = 0;
129
unsigned short vitesse;
130
int num_piste;                                                
131
int rpiste;
127 132

  
133
CanFrame vit_roue; 
128 134

  
129

  
130
CanFrame vit_roue; //canal de commande de la vitesse
131

  
132
void demarrer()
135
void demarrer(vitesse)
133 136
{
134 137
	vit_roue.data.id='V'; 
135 138
	vit_roue.data.rtr=0; 
136 139
	vit_roue.data.val=vitesse;
137 140
	snd_dtq (CanTx,vit_roue.msg);
138
	dly_tsk(100);
141
	
139 142
}
140 143

  
141 144
void asserv_tourelle()
142 145
{
143 146
        CanFrame comm;
147
		UINT flag;
144 148
		short angle;
145
		CanFrame req, reponse;
146
                        
147
       	while(1){      
148
	        req.data.id = 'R';                                
149
	        req.data.rtr = 1;       
150
			
151
			snd_dtq(CanTx, req.msg);
152
                
149
		short consigne = 450;
150
		CanFrame req;
151
        req.data.id = 'R';                              
152
	    req.data.rtr = 1;            
153
       	while(1){           
154
			periph[ADDR('R')].ev=0X01;
155
            snd_dtq(CanTx, req.msg);
156
            wai_flg(ev_periph,0X01,TWF_ANDW, &flag);
153 157
			angle = periph[ADDR('R')].val; //r?ponse de l'angle
154 158
		
155 159
			comm.data.id = 'T';                                                                
156 160
	        comm.data.rtr = 0;                                                                 
157
	        comm.data.val = k*(450 - angle);        
161
	        comm.data.val = k1*(consigne - angle);      
158 162
	        snd_dtq(CanTx, comm.msg);
159
			dly_tsk(100);
163
			dly_tsk(5);
160 164
		}
161 165
}
162 166

  
......
164 168
{
165 169
        CanFrame comm;
166 170
		CanFrame req, reponse;
167
		unsigned short consigne = 710;
168
		unsigned short distance;
171
		UINT flag;
172
		short consigne = 710;
173
		short distance;
169 174
		short rot;
170
               
175
        req.data.id = 'U';                               
176
		req.data.rtr = 1;  
171 177
		
172
     	while(1){
173
			
174
		        req.data.id = 'U';                                
175
		        req.data.rtr = 1;       
176
		
177
				snd_dtq(CanTx, req.msg);
178
		        periph[ADDR('U')].val;         
179
					
178
     	while(1){     
179
				periph[ADDR('U')].ev=0X02;
180
				snd_dtq(CanTx, req.msg);        
181
				wai_flg(ev_periph,0X02,TWF_ANDW, &flag);
180 182
				distance = periph[ADDR('U')].val; 
181
				rot = k2*(distance-consigne);
182
				if (distance <= 1000 && distance >= 400 )
183
	                {
184
	                	comm.data.id='D'; comm.data.rtr=0; comm.data.val=rot;
185
	                	snd_dtq (CanTx,comm.msg);
186
	                }
187
        		else
188
                {
189
                	comm.data.id='D'; comm.data.rtr=0; comm.data.val=0;
190
                	snd_dtq (CanTx,comm.msg);
191
				}                      
183
				
184
				if(distance>1500 || stop==1){
185
                 	comm.data.id='D';
186
                    comm.data.rtr = 0;
187
                    comm.data.val = 0;
188
                    dly_tsk(10);
189
				}
190
				else{
191
                                        
192
                	req.data.id = 'U';
193
                    req.data.rtr = 1;
194
                    snd_dtq(CanTx, req.msg);
195
                    distance=periph[ADDR('U')].val; 
196
                    //Asservissement
197
                    rot=k2*(distance-consigne);
198
                    comm.data.id='D';
199
                    comm.data.rtr = 0;
200
                    comm.data.val = rot;
201
        
202
                   snd_dtq(CanTx,comm.msg);
203
                   dly_tsk(10);
204
				}            
192 205
				dly_tsk(10);
193
				
194
				
195 206
		}
196 207
}
197 208

  
209

  
210
void capteur(){
211
        CanFrame req;
212
        CanFrame comm;
213
        while (1){
214
			stop = 0;
215
        	req.data.id = 'C';
216
            req.data.rtr = 1;
217
            snd_dtq(CanTx, req.msg);
218
            capt=periph[ADDR('C')].val;
219
			stop = 0;
220
            while(capt==0x7620){
221
				stop = 1;
222
				req.data.id = 'C';
223
                req.data.rtr = 1;
224
                snd_dtq(CanTx, req.msg);
225
                capt=periph[ADDR('C')].val;
226
                demarrer(50);
227
                comm.data.id='D';
228
                comm.data.rtr = 0;
229
                comm.data.val = 0;
230
                snd_dtq(CanTx,comm.msg);
231
				dly_tsk(1000);
232
			} 
233
         dly_tsk(10);
234
        }
235
}
236
				
237
void piste(){
238
        CanFrame requete;
239
        UINT flag;
240
        
241
        requete.data.id = 'M';
242
        requete.data.rtr = 1;        
243
        periph[ADDR('M')].ev = 0x04;
244
        snd_dtq(CanTx, requete.msg);
245
        wai_flg(ev_periph, 0x04, TWF_ANDW, &flag); 
246
        num_piste = periph[ADDR('M')].val;      
247
        rpiste = num_piste & 0x00ff;        
248
}				
249
			
250

  
251

  
198 252
void main()
199 253
{        
200 254
    ports_mcu();
......
210 264
	sta_tsk(ID_asserv_tourelle);
211 265
	dly_tsk(1000);
212 266
	sta_tsk(ID_asserv2);
267
	sta_tsk(ID_capteur);
213 268
	dly_tsk(2000);
269
	demarrer(30);
214 270
	while(1){
215
		demarrer();
216
        }
271
		
272
        dly_tsk(2000);	
273
    }
217 274
         
218 275
}
219 276
void acqui()

Also available in: Unified diff