RaydiumWikiNi

DetailfonctionPeriph

PagePrincipale :: DerniersChangements :: ParametresUtilisateur :: Vous êtes ec2-35-170-81-62.compute-1.amazonaws.com

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