Révision 79
Ajouté par maporte3 il y a plus de 6 ans
| branch/porte/Emb_App/programme_principal_etud.c | ||
|---|---|---|
|
//'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
|
||
|
// correspondant au nom du v?hicule si le nom se termine par '*'
|
||
|
|
||
|
unsigned int alpha=0;//prend la valeur de l'angle au temps t-1
|
||
|
unsigned int Angle_T=0;//prend la valeur de l'angle au temps t-1
|
||
|
unsigned int Consigne_T=500; //Consigne de l'angle de la tourelle
|
||
|
unsigned int valeur=0; //valeur transmise corrig?
|
||
|
unsigned int Somme_Erreur=0, Erreur=0, Delta_Erreur=0, Erreur_pre=0;
|
||
|
int Kp=1.5, Kpr=1.6;
|
||
|
int Kp=1.5, Kpr=1.6; //Param?tre des PID tourelle et roues
|
||
|
int Ki=4, Kir=10;
|
||
|
int Kd=1.5, Kdr=1;
|
||
|
unsigned int Vitesse=42,Distance;
|
||
|
unsigned int Angle_R=0;
|
||
|
unsigned int Angle_R=0; //Angle des roues
|
||
|
unsigned int ErreurRoues=0, ErreurRouesPre=0, Delta_ErreurRoues=0,Somme_ErreurRoues=0;
|
||
|
|
||
|
void Asserv_T(){
|
||
| ... | ... | |
|
demande.data.rtr=1;
|
||
|
|
||
|
snd_dtq (CanTx,demande.msg); // Interrogation du peripherique
|
||
|
alpha=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
|
||
|
Angle_T=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
|
||
|
|
||
|
comm.data.id='T';
|
||
|
comm.data.rtr=0;
|
||
| ... | ... | |
|
dly_tsk(2600);
|
||
|
comm.data.id='V'; //Commande de l'angle des roues
|
||
|
comm.data.rtr=0;
|
||
|
comm.data.val=Vitesse;
|
||
|
snd_dtq (CanTx,comm.msg);//Envoi de la commande
|
||
|
comm.data.val=Vitesse;
|
||
|
snd_dtq (CanTx,comm.msg);//Envoi de la commande de vitesse
|
||
|
|
||
|
dly_tsk(500);
|
||
|
}
|
||
| ... | ... | |
|
void Asserv_T_hc(){
|
||
|
|
||
|
Erreur_pre=Erreur;
|
||
|
Erreur=Consigne_T-alpha;
|
||
|
Erreur=Consigne_T-Angle_T;
|
||
|
Delta_Erreur=Erreur-Erreur_pre;
|
||
|
Somme_Erreur+=Erreur;
|
||
|
|
||
|
if(alpha!=Consigne_T)
|
||
|
valeur=Kp*Erreur+Kd*Delta_Erreur+(1/Ki)*Somme_Erreur;//correction de l'angle par handler cyclique
|
||
|
if(Angle_T!=Consigne_T)
|
||
|
valeur=Kp*Erreur+Kd*Delta_Erreur+(1/Ki)*Somme_Erreur;//correction de l'angle par handler cyclique de la tourelle
|
||
|
}
|
||
|
|
||
|
void Asserv_R_hc(){
|
||
| ... | ... | |
|
Somme_ErreurRoues+=ErreurRoues;
|
||
|
|
||
|
if(Distance!=660)
|
||
|
Angle_R=-Kpr*ErreurRoues+Kdr*Delta_Erreur+(1/Kir)*Somme_ErreurRoues;
|
||
|
Angle_R=-Kpr*ErreurRoues+Kdr*Delta_Erreur+(1/Kir)*Somme_ErreurRoues;//correction de l'angle par handler cyclique des roues
|
||
|
}
|
||
|
|
||
|
void main()
|
||
Formats disponibles : Unified diff
Ajout de commentaires et nom de variables plus parlante