Fonction periph_write() :
Retour
void periph_write (char addr,int x)
{
Periph * ptperiph=periphs;
float y;
while (ptperiph->adresse)
{
if (ptperiph->adresse !=addr)
{
ptperiph++;
continue;
}
y=ptperiph->a*x+ptperiph->b;
switch (ptperiph->type)
{
/** Activation/Désactivation de la traction du drone **/
case 'T':
{
if(x == 10) mot = 1; //Activation
if(x == 20) mot = 0; //Désactivation
}
break;
/******************************************************************************************
/** Pour pouvoir recevoir des nombres négatifs, les angles ont été reformatés **/
/** Pour un angle de -0.8 radian, la valeur d’entrée doit être de 0. **/
/** Proportionnellement, pour un angle de 0 radian, la valeur d’entrée doit être de 80. **/
/** Proportionnellement, pour un angle de 0.8 radian, la valeur d’entrée doit être de 160. **/
/******************************************************************************************
/** Servo moteur lié au volet de l’aile droite **/
case 'U':
{
if(x==80) alpha0aile1 = 0; //Remise à zéro du coefficient
if((x>0)&&(x<160)&&(x!=80)) alpha0aile1 = (((float)x-80)/100);
}
break;
/** Servo moteur lié au volet de l’aile gauche **/
case 'O':
{
if(x==80) alpha0aile2 = 0; //Remise à zéro du coefficient
if((x>0)&&(x<160)&&(x!=80)) alpha0aile2 = (((float)x-80)/100);
}
break;
/** Servo moteur lié aux deux volets **/
case 'D':
{
if(x==80)
{
alpha0aile2 = 0; //Remise à zéro des coefficients
alpha0aile1 = 0;
}
if((x>0)&&(x<160)&&(x!=80))
{
alpha0aile2 = -(((float)x-80)/100); //Inversement de l’actions sur
alpha0aile1 = (((float)x-80)/100); //les deux ailes.
}
}
break;
/** Servo moteur lié au volet empennage monter/descente **/
case 'W':
{
if(x==80) alpha0emp = 0; //Remise à zéro du coefficient
if((x>0)&&(x<160)&&(x!=80)) alpha0emp = (((float)x-80)/100);
}
break;
/** Servo moteur lié au volet empennage gauche/droite **/
case 'B':
{
if(x==80) alpha0emp1 = 0; //Remise à zéro du coefficient
if((x>0)&&(x<160)&&(x!=80)) alpha0emp1 = (((float)x-80)/100);
}
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