Révision 85
Ajouté par tibacher il y a plus de 6 ans
| branch/bacher/Emb_App/programme_principal_etud.c | ||
|---|---|---|
|
|
||
|
int cons=450; //contient la consigne
|
||
|
int retour=0; //contient la valeur retourner par le p?riph
|
||
|
int k=1; //contient le gain du correcteur
|
||
|
int k2=1;
|
||
|
int vitesse=75;
|
||
|
int vitesse_virage=30;
|
||
|
int k=2; //contient le gain du correcteur de la boucle de r?gulation de la tourelle
|
||
|
int k2=1; //contient le gain du correcteur de l'angle des roues
|
||
|
int vitesse=80; //vitesse en ligne droite
|
||
|
int vitesse_virage=30; //vitesse dans les virages
|
||
|
int valeur=0; //contient la valeur ? envoyer
|
||
|
int distance=0;
|
||
|
int cons_roue=700;
|
||
|
int ang_roue=0;
|
||
|
int distance=0; //mesure de la distance gr?ce au t?l?m?re
|
||
|
int cons_roue=700; //consigne du placement au milieu de la route
|
||
|
int ang_roue=0; //correction de l'angle ? effectuer
|
||
|
|
||
|
CanFrame vit_roue;
|
||
|
CanFrame requete;
|
||
|
CanFrame dist;
|
||
|
CanFrame rot_roue;
|
||
|
CanFrame vit_roue; //canal de commande de la vitesse
|
||
|
|
||
|
void demarre()
|
||
|
{
|
||
| ... | ... | |
|
|
||
|
req.data.id='R';
|
||
|
req.data.rtr=1;
|
||
|
|
||
|
|
||
|
snd_dtq (CanTx,req.msg); // Interrogation du peripherique
|
||
|
retour=periph[ADDR('R')].val; // contient la valeur de retour du simulateur.
|
||
|
|
||
| ... | ... | |
|
comm.data.val=valeur;
|
||
|
snd_dtq (CanTx,comm.msg); //on envoie la modification ? effectuer
|
||
|
if(retour!=cons) valeur=k*(cons-retour);
|
||
|
dly_tsk(5);
|
||
|
dly_tsk(3);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
int calcul_virage(int a){
|
||
|
if(a>0&&a<50) return -a/2+80;
|
||
|
else if(a<0&&a>-50) return a/2+80;
|
||
|
else if (a<-100||a>100){
|
||
|
return 30;
|
||
|
}
|
||
|
else{
|
||
|
return ((-(a*a)/100)+vitesse);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void mes_dist(){
|
||
|
|
||
|
CanFrame req;
|
||
|
UINT flag;
|
||
|
CanFrame req;
|
||
|
UINT flag;
|
||
|
while(1){
|
||
|
req.data.id='U';
|
||
|
req.data.rtr=1;
|
||
| ... | ... | |
|
snd_dtq(CanTx, req.msg);
|
||
|
|
||
|
wai_flg (ev_periph, 0x02, TWF_ANDW, &flag);
|
||
|
if (periph[ADDR('U')].val>1000){
|
||
|
if (periph[ADDR('U')].val>900){
|
||
|
}
|
||
|
else {
|
||
|
distance=periph[ADDR('U')].val;
|
||
|
}
|
||
|
dly_tsk(5);
|
||
|
dly_tsk(3);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void angle_roue()
|
||
|
{
|
||
|
|
||
|
CanFrame req,comm;
|
||
|
UINT flag;
|
||
|
while (1) {
|
||
|
CanFrame req,comm;
|
||
|
UINT flag;
|
||
|
while (1) {
|
||
|
req.data.id='D';
|
||
|
req.data.rtr=1;
|
||
|
periph[ADDR('D')].ev=0x01;
|
||
|
snd_dtq(CanTx, req.msg);
|
||
|
|
||
|
wai_flg (ev_periph, 0x01, TWF_ANDW, &flag);
|
||
|
wai_flg (ev_periph, 0x01, TWF_ANDW, &flag);
|
||
|
ang_roue=periph[ADDR('D')].val;
|
||
|
|
||
|
comm.data.id='D';
|
||
|
comm.data.rtr=0;
|
||
|
comm.data.val=k2*(distance-cons_roue);
|
||
|
if(k2*(distance-cons_roue)>30||k2*(distance-cons_roue)<-30){
|
||
|
vit_roue.data.val=vitesse_virage;
|
||
|
}
|
||
|
else vit_roue.data.val=vitesse;
|
||
|
comm.data.val=k2*(distance-cons_roue);
|
||
|
vit_roue.data.val=calcul_virage(k2*(distance-cons_roue));
|
||
|
snd_dtq (CanTx,vit_roue.msg);
|
||
|
snd_dtq(CanTx, comm.msg);
|
||
|
dly_tsk(5);
|
||
|
dly_tsk(3);
|
||
|
}
|
||
|
}
|
||
|
|
||
| ... | ... | |
|
sta_tsk(ID_angle_roue);
|
||
|
sta_tsk(ID_demarre);
|
||
|
|
||
|
while(1)
|
||
|
{ }
|
||
|
while(1){
|
||
|
}
|
||
|
|
||
|
}
|
||
|
|
||
|
void acqui()
|
||
Formats disponibles : Unified diff
ajout d'une fonction qui calcule la vitesse à appliquer en fonction des virages