Revision 55 branch/porte/Emb_App/programme_principal_etud.c

View differences:

programme_principal_etud.c
127 127
int Kp=2;
128 128
int Ki=7;
129 129
int Kd=2;
130
unsigned int Vitesse=2,Distance;
131
unsigned int Angle_R=0;
132
unsigned int ErreurRoues=0;
130 133

  
131 134
void Asserv_T(){
132 135
	
......
154 157
		}
155 158
	}
156 159

  
160
void Asserv_R(){
161
	
162
	//'D'/68/0x44?: Commande de l'angle des roues directrices (en 1/10 de degre).
163
	//'V'/86/0x56?: Commande en vitesse des roues motrices du vehicule (en radian /secondes).
164
	//'U'/85/0x55?: Distance mesuree par le telemetre (1/100 de metre).
165
	CanFrame comm;
166
	CanFrame demande;
167
	CanFrame reponse;
168
	while(1){
169
			
170
		demande.data.id='U'; 
171
		demande.data.rtr=1;
172
	
173
		snd_dtq (CanTx,demande.msg); // Interrogation du peripherique
174
		rcv_dtq (CanRx,&reponse.msg); // Attente de la reponse
175
		Distance=reponse.data.val; // contient la valeur de retour du simulateur.
176
		
177
		if(Distance<=680)
178
			Angle_R=-50;
179
		if(Distance>=730)
180
			Angle_R=50;
181
		
182
		comm.data.id='D'; //Commande de l'angle des roues
183
		comm.data.rtr=0; 
184
		comm.data.val=Angle_R;
185
		snd_dtq (CanTx,comm.msg);//Envoi de la commande
186
				
187
		dly_tsk(150);
188
		}
189
	}
190

  
157 191
void Asserv_T_hc(){
158 192
	Erreur_pre=Erreur;
159 193
	Erreur=Consigne_T-alpha;
......
163 197
	if(alpha!=Consigne_T)
164 198
		valeur=Kp*Erreur+Kd*Delta_Erreur+(1/Ki)*Somme_Erreur;//correction de l'angle par handler cyclique
165 199
	}
200

  
201
void Asserv_R_hc(){	
202
	
203
	if(alpha!=Consigne_T)
204
		Angle_R=Kp*ErreurRoues;//correction de l'angle par handler cyclique
205
	}
166 206

  
167 207
void main()
168 208
{
......
178 218
	sta_cyc(ID_acqui);
179 219
//	sta_tsk(ID_periph_rx);
180 220
	sta_tsk(ID_Asserv_T);
181
	sta_cyc(ID_Asserv_T_hc);	
221
	sta_cyc(ID_Asserv_T_hc);
222
	sta_tsk(ID_Asserv_R);
223
	sta_cyc(ID_Asserv_R_hc);
182 224
	
183 225

  
184 226
    while(1)

Also available in: Unified diff