Révision 201
Ajouté par audercoux il y a plus de 6 ans
| branch/decroux/Emb_App/programme_principal_etud.c | ||
|---|---|---|
|
|
||
|
unsigned int alpha;
|
||
|
int k=1;
|
||
|
int dis;
|
||
|
unsigned int distance;
|
||
|
|
||
|
|
||
|
void asservissemt_tourelle(void)
|
||
|
|
||
|
|
||
|
// Tache qui met la tourelle a 45degres
|
||
|
void asservissement_tourelle(void)
|
||
|
{
|
||
|
CanFrame comm;
|
||
|
CanFrame requete;
|
||
|
CanFrame reponse;
|
||
|
comm.data.id='T'; //Commade en vitesse de la tourelle poant le t?l?metre
|
||
|
comm.data.rtr=0;
|
||
|
comm.data.val=100;
|
||
|
snd_dtq (CanTx,comm.msg);
|
||
|
requete.data.id='R'; //Lecture de l'angle effecif de la tourelle portant le t?l?mtre
|
||
|
requete.data.rtr=1; // Indiquation d'une requ?te d'?criture
|
||
|
snd_dtq (CanTx,requete.msg); // Interrogation du peripherique
|
||
|
rcv_dtq (CanRx,&reponse.msg); // Attente de la reponse
|
||
|
alpha=reponse.data.val; // contient la valeur de retour du simulateur.
|
||
|
if(alpha!=450)
|
||
|
{
|
||
|
comm.data.id='T'; //Commade en vitesse de la tourelle poant le t?l?metre
|
||
|
comm.data.rtr=0;
|
||
|
comm.data.val=k*(450-alpha);
|
||
|
snd_dtq (CanTx,comm.msg);
|
||
|
}
|
||
|
CanFrame commTourel;
|
||
|
CanFrame requeteTourel;
|
||
|
CanFrame reponseTourel;
|
||
|
commTourel.data.id='T'; //Commade en vitesse de la tourelle portant le t?l?metre
|
||
|
commTourel.data.rtr=0;
|
||
|
commTourel.data.val=300;
|
||
|
snd_dtq (CanTx,commTourel.msg);
|
||
|
// Lectre de l'angle
|
||
|
requeteTourel.data.id='R'; //Lecture de l'angle effecif de la tourelle portant le t?l?mtre
|
||
|
requeteTourel.data.rtr=1; // Indiquation d'une requ?te de lecture
|
||
|
snd_dtq (CanTx,requeteTourel.msg); // Interrogation du peripherique
|
||
|
alpha=periph[ADDR('R')].val; //Contient la derniere valeur renvoyee par le simulateur
|
||
|
periph[ADDR('R')].maj; // Incremente a chaque reception
|
||
|
//Corection de l'angle
|
||
|
commTourel.data.id='T'; //Commade en vitesse de la tourelle poant le t?l?metre
|
||
|
commTourel.data.rtr=0;
|
||
|
commTourel.data.val=k*(450-alpha); //Corecteur propotionel
|
||
|
snd_dtq (CanTx,commTourel.msg);
|
||
|
}
|
||
|
|
||
|
void mesure_distance(void)
|
||
|
// Mesure la distance avec le t?l?metre
|
||
|
int mesure_distance(void)
|
||
|
{
|
||
|
CanFrame requete;
|
||
|
CanFrame reponse;
|
||
|
requete.data.id='U'; //Distance mesur?e par le t?l?m?tre
|
||
|
requete.data.rtr=1; // Indiquation d'une requ?te d'?criture
|
||
|
snd_dtq (CanTx,requete.msg); // Interrogation du peripherique
|
||
|
rcv_dtq (CanRx,&reponse.msg); // Attente de la reponse
|
||
|
dis=reponse.data.val; // contient la valeur de retour du simulateur.
|
||
|
CanFrame requeteDis;
|
||
|
CanFrame reponseDis;
|
||
|
unsigned int dis;
|
||
|
requeteDis.data.id='U'; //Distance mesur?e par le t?l?m?tre
|
||
|
requeteDis.data.rtr=1; // Indiquation d'une requiete de lecture
|
||
|
snd_dtq (CanTx,requeteDis.msg); // Interrogation du peripherique
|
||
|
dis=periph[ADDR('U')].val; // Contient la derniere valeur renvoyee par le simulateur
|
||
|
periph[ADDR('U')].maj; // Incremente a chaque reception
|
||
|
return dis;
|
||
|
}
|
||
|
|
||
|
// Asservissemet des roues
|
||
|
void asservissement_roues(int dis)
|
||
|
{
|
||
|
CanFrame commRoues;
|
||
|
commRoues.data.id='D'; // Commande de l'angle des roues
|
||
|
commRoues.data.rtr=0; //Indique une ?criture
|
||
|
commRoues.data.val= 0; // En ligen droite
|
||
|
snd_dtq(CanTx, commRoues.msg);
|
||
|
commRoues.data.id='D'; // Commande de l'angle des roues
|
||
|
commRoues.data.rtr=0; //Indique une requete d'?criture
|
||
|
commRoues.data.val= k*(dis-700); // Correction de la disance
|
||
|
snd_dtq(CanTx, commRoues.msg);
|
||
|
|
||
|
}
|
||
|
|
||
|
// Focntion delay
|
||
|
void Pause(void)
|
||
|
{
|
||
|
int i;
|
||
|
for(i=0; i<9000; i=i+1);
|
||
|
}
|
||
|
|
||
|
|
||
|
void main()
|
||
|
{
|
||
|
|
||
|
CanFrame commVit;
|
||
|
ports_mcu();
|
||
|
lcd_init();
|
||
|
periph_init();
|
||
| ... | ... | |
|
can_init();
|
||
|
clavier_init(1);
|
||
|
capture_init();
|
||
|
|
||
|
|
||
|
sta_cyc(ID_acqui);
|
||
|
// sta_tsk(ID_periph_rx);
|
||
|
sta_tsk(ID_periph_rx);
|
||
|
|
||
|
while(1)
|
||
|
{
|
||
|
//CanFrame vit;
|
||
|
asservissemt_tourelle();
|
||
|
mesure_distance();
|
||
|
//vit.data.id='V'; //Commande en vitesse des rous moicesd v?icule
|
||
|
//vit.data.rtr=0;
|
||
|
//vit.data.val=100;
|
||
|
//snd_dtq (CanTx,vit.msg);
|
||
|
|
||
|
asservissement_tourelle();
|
||
|
distance = mesure_distance();
|
||
|
asservissement_roues(distance);
|
||
|
//Commande en vitesse
|
||
|
dly_tsk(100);
|
||
|
commVit.data.id='V';
|
||
|
commVit.data.rtr=0;
|
||
|
commVit.data.val=5;
|
||
|
snd_dtq (CanTx,commVit.msg);
|
||
|
LED_J=1;
|
||
|
dly_tsk(100);
|
||
|
|
||
|
LED_J=0;
|
||
|
dly_tsk(100);
|
||
|
}
|
||
Formats disponibles : Unified diff
Passage du circuit vert fait mais très lentement