Fonction periph_write() :
Retour
void periph_write (char addr,int xin)
{
Periph * ptperiph=periphs;
float x;
while (ptperiph->adresse){
if (ptperiph->adresse !=addr) {
ptperiph++;
continue;
}
x=ptperiph->a*xin+ptperiph->b;
switch (ptperiph->type) //reapartition en fonction du
// peripherique appelee
{
case 'T': //Activation/Desactivation de la
// traction du drone
{
if(x > 1100)//Activation du moteur
mot = 1;
else
mot = 0;//Desactivation du moteur
if ( x > 1000 && x < 2000)
coef_moteur = 1-((x - 1000)/1000); // variation de la traction du moteur comprise entre 0 et 1
else coef_moteur = 1;
}
break;
case 'U': //Servo moteur lié au volet de
// l’aile droite
{
if(x==80) alpha0aile1 = 0; //pour mettre l'angle a 0, la
//valeur d'entree doit etre de 80
//x est divisee par 100 et soustrait de 80 donc pour mettre
// l'angle a 0.8, la valeur d'entree doit etre de 160
if((x>0)&&(x<160)&&(x!=80)) alpha0aile1 = (((float)x-80)/80);
}
break;
case 'O': //Servo moteur lié au volet de
// l’aile gauche
{
if(x==80) alpha0aile2 = 0; //pour mettre l'angle a 0, la
//valeur d'entree doit etre de 80
//x est divisee par 100 et soustrait de 80 donc pour mettre
// l'angle a 0.8, la valeur d'entree doit etre de 160
if((x>0)&&(x<160)&&(x!=80)) alpha0aile2 = (((float)x-80)/80);
}
break;
case 'D': //Commande des 2 Servos moteur lié
// au 2 volets
{
//pour mettre les angles a 0, la valeur d'entree doit etre de 80
if ( (x >= 70) && (x <= 90) )
{
alpha0aile2 = 0;
alpha0aile1 = 0;
}
//Les angles sont inversees pour permettre de faire virer
// l'avion correctement
if ( (x > 0) && (x < 160) && !( (x >= 75) && (x <= 85) ))
{
//x est divisee par 100 et soustrait de 80 donc pour
// mettre l'angle a 0.8, la valeur d'entree doit
// etre de 160
alpha0aile2 = -(((float)x-80)/100);
alpha0aile1 = (((float)x-80)/100);
}
break;
}
case 'W': //Servo moteur lié au volet
//empennage monter/descente
{
if ( (x >= 70) && (x <= 90) )
alpha0emp = -0.08; //pour mettre l'angle a 0, la
//valeur d'entree doit etre de 80
//x est divisee par 200 et soustrait de 80 donc pour
// mettre l'angle a 0.4, la valeur d'entree doit
// etre de 160
if ( (x > 0) && (x < 160) && !( (x >= 70) && (x <= 90) ))
alpha0emp = (((float)x-80)/200)-0.05; //-0.05 pour plus ou moins maintenir l'assiette sans toucher a rien
}
break;
case 'B': //Servo moteur lié au volet
//empennage gauche/droite
{
if ( (x >= 60) && (x <= 100) )
alpha0emp1 = 0; //pour mettre l'angle a 0, la
//valeur d'entree doit etre de 80
//x est divisee par 80 et soustrait de 80 donc pour
// mettre l'angle a 1, la valeur d'entree doit
// etre de 160
if ( (x > 0) && (x < 160) && !( (x >= 60) && (x <= 100) ))
alpha0emp1 = -(((float)x-80)/80);
}
break;
}
break;
}
}
Fonction periph_read() :
int periph_read (char addr)
{
Periph * ptperiph=periphs;
int transf;
dReal rx;
dReal ry;
dReal rz;
while (ptperiph->adresse)
{
if (ptperiph->adresse !=addr)
{
ptperiph++;
continue;
}
switch (ptperiph->type)
{
/** Position en X par rapport à la carte **/
case 'X':
{
dReal * pos;
pos = raydium_ode_element_pos_get_name(ptperiph->ray_nom);//récuperation
transf=pos[0]*ptperiph->a+ptperiph->b; //multiplication et ajout des coeff
if(transf<0){transf=65536+transf;} //nombre négatif en retour de fonction
}
break;
/** Position en Y par rapport à la carte **/
case 'Y':
{
dReal * pos;
pos = raydium_ode_element_pos_get_name(ptperiph->ray_nom); //récupération
transf=pos[1]*ptperiph->a+ptperiph->b; //multiplication et ajout des coeff
if(transf<0){transf=65536+transf;} //nombre négatif en retour de fonction
}
break;
/** Position en Z par rapport à la carte **/
case 'Z':
{
dReal * pos;
pos = raydium_ode_element_pos_get_name(ptperiph->ray_nom); //récupération
transf=pos[2]*ptperiph->a+ptperiph->b; //multiplication et ajout des coeff
if(transf<0){transf=65536+transf;} //nombre négatif en retour de fonction
}
break;
/** Quaternion 1, AXE X (angle en dégrée) **/
case 'F':
{
transf=vx; //récupération de la variable globale contenant l'angle X
if(transf<0){transf=65536+transf;} //nombre négatif en retour de fonction
}
break;
/** Quaternion 2, AXE Y (angle en dégrée) **/
case 'G':
{
transf=vy; //récupération de la variable globale contenant l'angle Y
if(transf<0){transf=65536+transf;} //nombre négatif en retour de fonction
}
break;
/** Quaternion 3, AXE Z (angle en dégrée) **/
case 'H':
{
transf=vz;//recuperation de la variable globale contenant l'angle Z
if(transf<0){transf=65536+transf;}//nombre négatif en retour de fonction
}
break;
}
break;
}
return transf;
}
Les variables alpha0emp1, alpha0emp … sont déclarée en globales et sont défini dans la partie
EquaPhys
La variable tableau « pos » récupère la position du drone sur la carte (x, y z). Si cette valeur est inférieure à
0, un calcul est réalisé pour renvoyé un nombre négatif au microcontrôleur (ajout de 65536).
Retour