Tour de table de Pilou

 Voici une vidéo plus longue montrant Pilou qui s'est engagé vers la salle et qui arrive à en faire le tour.

Parcours entièrement en mode autonome donc évitement des obstacles. 

Bien que ses détecteurs n'aiment pas les pieds de chaises ou de tables ou tout obstacle filiforme, Pilou s'en sort plutôt bien, au grand dam du preneur de vues qui n'avait pas prévu qu'il passe derrière la table.

Il se permet d'éviter habilement un balai et les roues des vélos pour aller se ficher lamentablement dans la machine à coudre.... Celle là a des pieds moins filiformes que les chaises.... " Va comprendre... Eugène " !!!

Eteignez les lumières et bonne séance 

On voudrait qu'il puisse faire visiter une maison à vendre!! !

Gilles 

Et un virage, un !

Une toute petite vidéo qui vous montrent le fonctionnement du pilotage automatique... Pilou fait ses premiers virages automatiquement pour éviter la collision avec les murs de la maison !!!!

Remarquez qu'il ne s'arrête pas pour décider d'un virage à prendre... il le fait tout en roulant

et voilà le travail 

Pilou

Voici les premiers pas de "Pilou", le nouveau robot équilibriste.

Encore hésitant sous le soleil de la terrasse, il peut aussi avancer, tourner, reculer et se promener en évitant les obstacles.

Regardez !  envie d'en faire un ? ou mieux ? les articles qui suivent sont là pour ca.

Le bruit ? ce sont ses 2 moteurs  alimentés en découpage à 100 Hz... 

Objet

La carte d'asservissement possède, outre le processeur d'odométrie, le processeur d'asservissement.

C'est lui qui pilote les moteurs par découpage donc est capable de leur appliquer à chacun une puissance variable en marche avant et arrière.

L'asservissement qui calcule cette puissance (par moteur) utilise en entrée :

les informations parvenant de la carte d'attitude : angle et vitesse angulaire

les information parvenant de l'odométrie (par roue) : distance et vitesse de déplacement

les information parvenant de la carte de pilotage : consignes de vitesse et de volant

Voici donc une description des deux bouts de codes importants... Le programme complet est bien plus conséquent mais est-ce utile de tout montrer alors que vous voulez sûrement construire VOTRE robot et non une copie.

 Commande des moteurs

Pour commander les moteurs, il faut générer un signal indiquant le sens de rotation et un signal périodique dont le rapport cyclique dépend de la puissance à appliquer.

Il faut noter ici qu'on compense par un seuil le rapport cyclique pour éviter que le moteur ne tourne pas dans les toutes petites puissances : zéro correspondra à un moteur arrêté mais epsilon correspondra à un rapport cyclique qui fait tourner le moteur. Voir ce site pour le détail.

Le contrôle des moteurs se fait en interruption :

L'interrupt timer 0  qui passe toutes les 5msec démarre alternativement chaque moteur toutes les 10 msec et lance les timers 1 ou 3, suivant la roue avec une temporisation égale à la puissance demandée.

Lorsque l'interrupt 1 ou 3 passe, on stoppe le moteur correspondant.

La fonction setPwm() permet de calculer la puiss    ance à appliquer avec tous les contrôles de validité des informations et application du sueil décrit pécédemment.

/******************************************************
/     Interrupt timers 0 , 1  et 3
/        traite le pwm des roues gauche et droite
/        toggle 10msec pour traiter alternativement gauche et droite
/         (égalise mieux les pointes de courants)
/        utilise timer 1 et 3  pour arrêter les impulsions
/       
/******************************************************/

#int_TIMER0
void TIMER0_isr() {                 // interruption toutes les 5msec.
    set_timer0(VAL_TMR0);             // restart timer
    toggle=!toggle;
    if (toggle){                      // Toutes les 10msec : roue avant
        toggleA=!toggleA;                              // toggle de lancement des fonctions de base
    if(toggleA)    GoBase=true;        // toutes les 20 msec
       
    output_low(D_ENABLE);
    if (Torque_D>0){                                // pair : roue droite
          output_high(D_H1);            //marche avant roue droite
          output_low(D_H2);
          set_timer1(65536 - Torque_D); // set timer 1
          output_high(D_ENABLE);        // enable roue droite
    }
    else if (Torque_D<0){
          output_low(D_H1);             //marche arrière roue droite
          output_high(D_H2);
          set_timer1(65536 +Torque_D);  // set timer 1
          output_high(D_ENABLE);        // enable roue droite
    }   

   } else {                         // Toutes les 10msec : roue gauche
        output_low(G_ENABLE);   
        if (Torque_G>0){
          output_high(G_H1);             //marche avant roue gauche
          output_low(G_H2);
          set_timer3(65536 - Torque_G);  // set timer 3
          output_high(G_ENABLE);         // enable roue gauche
     }
     else if (Torque_G<0){
          output_low(G_H1);              //marche arrière roue gauche
          output_high(G_H2);
          set_timer3(65536 +Torque_G);   // set timer 3
          output_high(G_ENABLE);         // enable roue gauche
     }
  }
}

//
//interrupt timer 1 : arrêter l'impulsion roue droite
#int_TIMER1
void TIMER1_isr(){
    output_low(D_ENABLE);
    set_timer1(0);
}

//
//interrupt timer 3 : arrêter l'impulsion roue gauche
#int_TIMER3
void TIMER3_isr(){
    output_low(G_ENABLE);
        set_timer3(0);
}

//
//init du pwm
void Init_Pwm(){
     Torque_D = 0;           // init puissance moteur droit
     Torque_G = 0;           // init puissance moteur gauche
     pwmtg=0;
     pwmtd=0;
     toggle=true;
     toggleA=true;
}

//
// set pwm
void setPwm(){
    if(pwmtd<0){
         pwmtd -=THRESH;                    // annulation du threshold
         if(pwmtd<-TQMAX)  pwmtd=-TQMAX;    // contrôle de non dépassement
    }else if(pwmtd>0){
          pwmtd +=THRESH;
          if(pwmtd> TQMAX)  pwmtd= TQMAX;
    }

    if(pwmtg<0){
         pwmtg -=THRESH;
         if(pwmtg<-TQMAX)  pwmtg=-TQMAX;
    }else if(pwmtg>0){
         pwmtg +=THRESH;
         if(pwmtg> TQMAX)  pwmtg= TQMAX;
    }
    disable_interrupts(GLOBAL);      // application des valeurs
    Torque_D = pwmtd;
    Torque_G = pwmtg;
    enable_interrupts(GLOBAL);
}

Formule de balancement

 Cette fonction répond par la puissance à appliquer sur chaque roue. Le code est court pour unr telle fonction. Bien sûr, il existe d'autres fonctions sui préparent les informations utiles...

Notez 4 points :

 Quand le robot est à l'arrêt, on utilise un asservissement en position au lieu de l'asservissement en vitesse

L'erreur entre position souhaitée et position réelle est écrétée pour que le robot rejoigne tranquillement sa consigne... D'une manière générale, toutes les valeurs sont contrôlées et bornées.

Le calcul Trim est destiné à faire pencher le robot progressivement quand il "attaque" une montée... Pour éviter qu'il ne s'écrase vers l'avant dès la côte ou l'obstacle terminé, cette valeur reprend très vite une valeur nulle ou négative.. Il y a lieu de faire des essais de coefficients pour bien régler ce terme Trim

Tous les coefficients sont modifiables, en fonctionnement, à partir de la télécommande infra rouge... Ceci est très utile pour montrer rapidement l'effet de chacun d'eux.. (bien entendu, il existe une commande de retour aux paramètres usine !)

Enfin, pour avoir un fonctionnement satisfaisant pour toutes les tensions de la batterie (de complètement chargée à  presque à plat), on applique au résultat un coefficient dépendant de l'écart tension-tension nominale.

/*********************************************************
/
/         Balancing : lancé par asserv tous les 20 msec
/
**********************************************************/
void balancing(){
    /*- en entrée de la routine, on a :
    *   Angle= l'angle en 500ièmes de degrés
    *   Vangle= la vitesse     angulaire en degrés par secondes  
    *   Vitesse_D et Vitesse_G : vitesse mesurée en tics par 50msec
    *   Position_Ecart = l'écart en 2*tics entre la position du robot et la consigne de position
    *   Orientation_Ecart= écart en tics entre la direction du robot et la consigne de direction
    *   Vitesse_Consigne = vitesse de consigne entre -20 et +20  (approx en cm par seconde)
    *    
    * - le but est de calculer les pwm des moteurs droit et gauche
    */   
         signed int32 Err_V;
          float Vit;
          // si la vitesse est nulle, alors asservissement en position
          if (Vitesse_Consigne==0)
                      Vit =((float)(Vitesse_D)+(float)(Vitesse_G) -  ((float)(Position_Ecart))/16.0);
              else
              // sinon, appliquer un facteur dépendant de l'écart vitesse réelle -vitesse de consigne
                   Vit =(float)(Vitesse_D)+(float)(Vitesse_G) -  (float)(Vitesse_Consigne);   

     if      ( Vit>30.0) Vit=30.0;             // plafonner l'écart à 30cm*s pour les facteurs K4 et K3
     else if (Vit<-30.0) Vit=-30.0;               // (vit est en mm(tics)/100msec => vit=1 ==> 1cm/sec

     Trim += K4B*Vit +K5B*(Vit-Vit_O);         // ajuster le trim d'asservissement
     if      (Trim>2000.0)  Trim=2000.0;       // plafonner le trim à quelques degrés
     else if (Trim<-2000.0) Trim=-2000.0;      
    
        Vit_O=Vit;                               // mémorisation de l'écart                          
       pwmtb  = K1B*(float)(Angle)              // proportionnel à l'angle K1B
             +K2B*(float)(Vangle)             // + amortissement en fonction de la vitesse angulaire
               +K3B*Vit                         // + vitesse
             +Trim;                           // et le trim d'asservissement en position
     // appliquer un coeff dépendant de la tension batterie
     pwmtb = pwmtb*Err_Voltage;                                          
     Err_V = (signed int32)(Orientation_Ecart)*5;  // et enfin, la consigne du volant à chaque roue    
     pwmtg=(signed int32)(pwmtb)+ Err_V;
     pwmtd=(signed int32)(pwmtb)- Err_V;
       setPwm();                                // appliquer le résultat

    }

 Conclusion

Le programme comprend d'autres fonction (retour à l'équikibre après chute, alarme tension faible, dialogue I2C, interruption de surintensité .. ) mais  ces deux fonctions constituent le coeur de l'asservissement.

Suite et Fin

Vous connaissez presque tout  déjà... Reste (outre le code de traitement des messages I2C que je ne décris pas ici) le filtrage Kalman et le choix des infos envoyées à l'asservissement....

Kalman

Pourquoi faire ??? 

Le gyroscope est excellent pour son temps de réponse et son bruitage quasi nul. Mais il ne donne pas l'angle... juste la vitesse angulaire. Il dérive

L'accéléromètre fournit un angle absolu. Mais il est très bruité (ne serait-ce que par les vibrations du moteur) et, en raison de l'intégration nécessaire pour diminuer ce bruit, a un temps de réponse trop lent, trop mou.

Il faut donc "mélanger" ces deux données.... un filtre eût été bien (passe bas pour l'accéléromètre et passe haut pour le gyro) mais j'ai voulu suivre un peu la mode en utilisant un filtre de Kalman. 

Le source C de référence est rotomation.c que vous trouverez en fouillant un peu sur internet. Il est superbement commenté  et  n'a posé aucun problème à intégrer. Il faut simplement songer à l'alimenter avec la vitesse angulaire de manière bien récurrente et lui injecter l'angle d'accéléromètre pour y récupérer l'angle calculé par le filtre. 

  Voici le code de la fonction

/*
 *    Kalman.c : source tirée de tilt.c de rotomation
 *
 * 1 dimensional tilt sensor using a dual axis accelerometer
 * and single axis angular rate gyro.  The two sensors are fused
 * via a two state Kalman filter, with one state being the angle
 * and the other state being the gyro bias.
 *
 * Gyro bias is automatically tracked by the filter.  This seems like magic.
 *
 */




/********************************************************************
 *
 *                  state_update (float Pitch gyro measurement)
 *
 ********************************************************************
 * state_update is called every dt with a biased gyro measurement
 * by the user of the module.  It updates the current angle and
 * rate estimate.
 *
 * The pitch gyro measurement should be scaled into real units, but
 * does not need any bias removal.  The filter will track the bias.
 *
 * Our state vector is:
 *        X = [ angle, gyro_bias ]
 * It runs the state estimation forward via the state functions:
 *       Xdot = [ angle_dot, gyro_bias_dot ]
 *       angle_dot    = gyro - gyro_bias
 *       gyro_bias_dot    = 0
 * And updates the covariance matrix via the function:
 *       Pdot = A*P + P*A' + Q
 * A is the Jacobian of Xdot with respect to the states:
 *       A = [ d(angle_dot)/d(angle)     d(angle_dot)/d(gyro_bias) ]
 *           [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
 *         = [ 0 -1 ]
 *           [ 0  0 ]
 *
 *
 * ENTREE : q_m =     Pitch gyro measurement
 */
void state_update(float q_m) {
    float q;
    float Pdot[2*2];

  q = q_m - Gyro_bias;     /* Unbias our gyro */

      // Compute the derivative of the covariance matrix
      //      Pdot = A*P + P*A' + Q
      // We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied
      // by P and P multiplied by A' = [ 0 0, -1, 0 ].  This is then added
      // to the diagonal elements of Q, which are Q_angle and Q_gyro.
    Pdot[0]= Q_angle - P[0][1] - P[1][0];    /* 0,0 */
    Pdot[1]=            - P[1][1];                /* 0,1 */
    Pdot[2]=            - P[1][1];                /* 1,0 */
    Pdot[3]= Q_gyro;                            /* 1,1 */

    Gyro_rate = q;      // Store our unbias gyro estimate

     /* Update our angle estimate
      * Angle_Kalman += angle_dot * dt
      *              += (gyro - gyro_bias) * dt
      *              += q * dt
      */
    Angle_Kalman += q * Filtrage_Period;

      /* Update the covariance matrix */
    P[0][0] += Pdot[0] * Filtrage_Period;
    P[0][1] += Pdot[1] * Filtrage_Period;
    P[1][0] += Pdot[2] * Filtrage_Period;
    P[1][1] += Pdot[3] * Filtrage_Period;
}


/********************************************************************
 *
 *              kalman_update
 *
 ********************************************************************
 * kalman_update is called by a user of the module when a new
 * accelerometer measurement is available.  ax_m and az_m do not
 * need to be scaled into actual units, but must be zeroed and have
 * the same scale.
 *
 * This does not need to be called every time step, but can be if
 * the accelerometer data are available at the same rate as the
 * rate gyro measurement.
 *
 * For a two-axis accelerometer mounted perpendicular to the rotation
 * axis, we can compute the angle for the full 360 degree rotation
 * with no linearization errors by using the arctangent of the two readings.
 *
 * The C matrix is a 1x2 (measurements x states) matrix that
 * is the Jacobian matrix of the measurement value with respect
 * to the states.  In this case, C is:
 *        C = [ d(angle_m)/d(angle)  d(angle_m)/d(gyro_bias) ]
 *          = [ 1 0 ]
 * because the angle measurement directly corresponds to the angle
 * estimate and the angle measurement has no relation to the gyro bias.
 *
 */

/*
void kalman_update(float ax_m, float az_m) {
      float angle_m;

*/
// routine lancée dès que le résultat de l'accéléromètre est connu
// paramètre d'entrée = l'angle mesuré par l'accéléromètre
float kalman_update(float angle_m){
      float angle_err;
      float C_0;
      float PCt_0;
      float PCt_1;
      float E;
      float K_0;
      float K_1;
      float t_0;
      float t_1;
        //Compute our measured angle and the error in our estimate
//    angle_m = atan2( -az_m, ax_m );
    angle_err = angle_m - Angle_Kalman;

     //C_0 shows how the state measurement directly relates to the state estimate.
     // The C_1 shows that the state measurement does not relate
     // to the gyro bias estimate.  We don't actually use this, so we comment it out.
      C_0 = 1;
      /*  float        C_1 = 0; */

    // PCt<2,1> = P<2,2> * C'<2,1>, which we use twice.  This makes
    // it worthwhile to precompute and store the two values.
    // Note that C[0,1] = C_1 is zero, so we do not compute that term.
     PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */
     PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */
       
    // Compute the error estimate.  From the Kalman filter paper:
    //    E = C P C' + R
    // Dimensionally,
    //    E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>
    // Again, note that C_1 is zero, so we do not compute the term.
      E =    R_angle    + C_0 * PCt_0    /*    + C_1 * PCt_1 = 0 */ ;

    // Compute the Kalman filter gains.  From the Kalman paper:
    //      K = P C' inv(E)
    // Dimensionally:
    //      K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>
    // Luckilly, E is <1,1>, so the inverse of E is just 1/E.
     K_0 = PCt_0 / E;
     K_1 = PCt_1 / E;
       
    /* Update covariance matrix.  Again, from the Kalman filter paper:
     *      P = P - K C P
     * Dimensionally:
     *      P<2,2> -= K<2,1> C<1,2> P<2,2>
     * We first compute t<1,2> = C P.  Note that:
     *      t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]
     * But, since C_1 is zero, we have:
     *      t[0,0] = C[0,0] * P[0,0] = PCt[0,0]
     * This saves us a floating point multiply.
     */
    t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
    t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 */

    P[0][0] -= K_0 * t_0;
    P[0][1] -= K_0 * t_1;
    P[1][0] -= K_1 * t_0;
    P[1][1] -= K_1 * t_1;
   
  //Update our state estimate.  Again, from the Kalman paper:
    //    X += K * err
    //And, dimensionally,
    //  X<2> = X<2> + K<2,1> * err<1,1>
    // err is a measurement of the difference in the measured state
    // and the estimate state.  In our case, it is just the difference
    // between the two accelerometer measured angle and our estimated angle.
    Angle_Kalman      += K_0 * angle_err;
    Gyro_bias    += K_1 * angle_err;
    return Angle_Kalman;
}

Les appels à cette fonction ont lieu  à chaque calcul :

appel à state_update à chaque calcule de la vitesse du gyro

appel à kalman_update (argument = angle mesuré ) pour calculer l'angle  réel

 Choix de la réponse envoyée à l'asservissement

En fonction du type de mesure demandée (avec la télécommande), on envoit à la carte d'asservissement

- Une vitesse angulaire toujours issue du gyroscope

-Un angle_K  fonction de la demande : 

Gyro + accéléromètre :

Angle_K= kalman_update(Moy_Angle_Accel);

Gyro + Opto

Angle_K= kalman_update(Angle_Opto);

Gyro seulement

Angle_K=Angle_Gyro +Trim_Angle_Gyro; 

Notez qu'on n'utilise pas le filtre de Kalman dans ce dernier cas. 

 Conclusion

 Voci terminé ce petit tour au milieu du code...Il  manque les fonctions de messagerie I2C et l'assemblage final avec les initialisations, les cadencements, les divers tests d'état de fonctionnement mais ce n'était pas le but de ces articles. Si toutefois vous voulez le code complet (.C et .H) demandez le moi.

Actuellement, le mode gyro+accéléromètre est très bruité par les vibration des moteurs et pratiquement inutilié. Le mode Gyro+Opto est d'un fonctionnent irréprochable et le mode Gyro seul est très souvent utilisé en démonstration... Il n'est pas encore complet : il faut en effet travailler les situations où le robot est non vertical et bloqué sur un obstacle (table, chaise) : en effet, l'algorithme qui dit qu'il est vertical va peut à peu dériver vers la situation réelle, ce qui provoque un retour au balancement périlleux. Encore un effort .... 

Enfin, il faut noter une fonction (non décrite ici) qui permet d'enregistrer 4  variables durant 3 secondes.... En branchant l'ICD en debug, on peut alors uploader ces données et , par export vers un tableur, on visualise les courbes : ceci m'a servi à comparer , au cours d'un balancement normal, les valeurs brutes, lissées de l'accéléromètre, de l'opto et du gyro... Une fonction qui vaut la peine d'être écrite car elle procure , en différé, un équivalent d'oscilloscope multicanaux...

Le code? quel code?

La carte  a été développée pour faire des essais avec plusieurs capteurs : un gyroscope, un accéléromètre et un système à télémètre infra rouge. Le code a donc été développé pour pouvoir faire diverses combinaisons. On peut même en changer à l'aide de la télécommande sans arrêter le robot.

Bien que le fonctionnement basé uniquement sur le gyroscope soit satisfaisant, on décrit içi le code complet.

La carte utilise un 18F4550 cadencé à 40 MHz 

Gyroscope

Pour traiter le gyroscope, il faut lui envoyer des signaux calibrés et les analyser en sortie. Le signal émis est une impulsion de 1msec toutes les 10msec.

On utilise le CCP2  en mode compare pour envoyer le signal. L'interruption est très simple :

/*******************************
/   Interrupt Compare
/******************************/
#INT_CCP2
void ccp2_isr (void) {
   if (input(GYRO_PWM)){    // si on envoie un 1  
      CCP_2 += SEND_ON;     // durée d'envoi d'un 1    
   } else {                               // si on envoie un zéro      
     CCP_2 += SEND_OFF;    // durée d'envoi d'un zéro   
  }         
}

 Noter qu'on ne réinitialise pas le compteur mais on se contente d'ajouter le temps à compter.

Simple...  Il faut maintenant analyser le signal en retour du gyroscope... Pour ce faire, on va utiliser CCP1  en mode capture pour avoir les temps au front montant puis descendant.. Le code en interrupt est le suivant (les explications suivent) :

/********************************
/    Interrupt capture
/*******************************/
#INT_CCP1
void ccp1_isr (void){
    Mauvais = true;                           // invalider le calcul de l'accéléromètre
    if (Wait_On){                             // réception d'un début d'impulsion
        GyroKo=false;
        Wait_On=false;
        Gyro_Debut= CCP_1;
        setup_ccp1(CCP_CAPTURE_FE);
     
  }else{                                         // réception d'une fin d'impulsion
      Wait_On=true;  
      Duree_A = CCP_1 - Gyro_Debut;
      if (Duree_A <0) Duree_A = -Duree_A;
      setup_ccp1(CCP_CAPTURE_RE);
//   
      if(Gyro_Toggle){          // premier passage
          Gyro_Toggle=false;
         if(!GyroKo)Duree_B= Duree_A;
         else Duree_B=Duree_Gyro/2;
        Accel_Flag=true;          // lancer l'accéléromètre 20msec
    }else{                    // second passage
        Gyro_Toggle = true;
        if(!GyroKo)Duree_Gyro = Duree_B+Duree_A;
        else Duree_Gyro = Duree_B +(Duree_Gyro/2);
        Flag_Traitement = true;      // lancer le traitement 20msec
    }   
  }
}

Le booléen Mauvais sert à invalider le calcul de l'accéléromètre (qui est perturbé par cette interruption).

Gyro_début mémorise la valeur du compteur de temps au front montant. Duree_A contient donc la durée de l'impulsion. 

On va faire 2 mesures et donner une réponse toutes les 20msec. Noter que GyroKo est levé quand une interruption I2C a eu lieu et donc on invalide la mesure en cours.

Enfin, pour maitriser le déroulement des taches, on lance (en base) le calcul de l'accéléromètre et le traitement de l'attitude toutes les 20msec mais chacun étant décalé de 10 msec par rapport à l'autre. 

Accéléromètre

  L'accéléromètre émet vers le micro deux (axes X et Y) trains de signaux dont le rapport cyclique permet de déduire l'accélération sur chaque axe.

La mesure est lancée en niveau de base toutes les 20msec (lors de la fin de la première mesure du gyro -voir ci dessus-). 

On ne va pas faire compliqué et simplement boucler pour calculer les durées respectives (par axe de mesure) du 1 et du 0 donc le rapport cyclique R . On calculera ensuite la valeur de l'accélération par une  équation linéaire de type AR+B. Les valeurs A et B sont calculées lors de la mise au point et vont correspondre à unevaleur de 1G (X tourné vers le sol) et 0G (X horizontal).

Pour avoir une mesure fiable, on annule toutes celles qui auraient été perturbées par des interruptions : on alloue donc au total 6  tentatives de mesures. 

Voici le code de la fonction de lecture de l'accéléromètre

void Lit_Accel() {
    int i;
    float G_Temp;
  Accel_X_OK=false;
  Accel_Z_OK=false;
    Accel_Flag= false;           // acquittement.
   
  for (i=0;i<5;i++){           // 5 essais de réception X max (on attend entre 1 et1.5msec par essai)
      Mauvais = FALSE;
      if (input(ACCEL_X)){         // si entrée = 1, on va mesurer OFF puis ON
          while(input(ACCEL_X));     // attente passage à 0
          Accel_1= get_timer3();     // date passage à 0
          while(!input(ACCEL_X));    // attente passage à 1
          Accel_2= get_timer3();    // date passage à 1
          Accel_X_OFF = Accel_2-Accel_1;
          while(input(ACCEL_X));   // attente passage à 0
          Accel_3= get_timer3();     // date passage à 0
          Accel_X_ON = Accel_3-Accel_2;
     }else{                        // si entrée=0, on va mesurer ON puis OFF
           while(!input(ACCEL_X));    // attente passage à 1
           Accel_1= get_timer3();     // date passage à 1   
           while(input(ACCEL_X));     // attente passage à 0
          Accel_2= get_timer3();    // date passage à 0               
          Accel_X_ON = Accel_2-Accel_1;
           while(!input(ACCEL_X));    // attente passage à 1
          Accel_3= get_timer3();     // date passage à 1
          Accel_X_OFF = Accel_3-Accel_2;
    }                
    if (Mauvais==false) {          // pas d'interruption : mesure bonne
       Accel_X_OK = true;
          break;                      // passage à Z
    }
  }
 
  for (i=i;i<6;i++){              //pas plus de 6 essais au total **********************
      Mauvais = FALSE;
      if (input(ACCEL_Z)){         // si entrée = 1, on va mesurer OFF puis ON
          while(input(ACCEL_Z));     // attente passage à 0
          Accel_1= get_timer3();     // date passage à 0
          while(!input(ACCEL_Z));    // attente passage à 1
          Accel_2= get_timer3();    // date passage à 1
          Accel_Z_OFF = Accel_2-Accel_1;
          while(input(ACCEL_Z));     // attente passage à 0
          Accel_3= get_timer3();     // date passage à 0
          Accel_Z_ON = Accel_3-Accel_2;
     }else{                        // si entrée=0, on va mesurer ON puis OFF
           while(!input(ACCEL_Z));    // attente passage à 1
           Accel_1= get_timer3();     // date passage à 1   
           while(input(ACCEL_Z));     // attente passage à 0
          Accel_2= get_timer3();    // date passage à 0               
          Accel_Z_ON = Accel_2-Accel_1;
           while(!input(ACCEL_Z));    // attente passage à 1
          Accel_3= get_timer3();     // date passage à 1
          Accel_Z_OFF = Accel_3-Accel_2;
    }                
    if (Mauvais==false) {          // pas d'interruption : mesure bonne
       Accel_Z_OK = true;
          break;                      // passage à Z
    }
  }
                                                                     
  if(Accel_X_OK && Accel_Z_OK){     // seulement en cas de succès!
      G_Temp= Ax*( ((float)(Accel_X_ON)/(float)(Accel_X_ON +Accel_X_OFF)) - Gx0);
      if (G_Temp>1) G_Temp=1;
      if (G_Temp<-1) G_Temp=-1;
           G_X=(G_X_Old3+G_X_Old2+G_X_Old1 + G_Temp)/4.0;
           G_X_Old3=G_X_Old2;
           G_X_Old2=G_X_Old1;
           G_X_Old1=G_Temp;


        G_Temp= Az*( ((float)(Accel_Z_ON)/(float)(Accel_Z_ON +Accel_Z_OFF)) - Gz0);
      if (G_Temp>1) G_Temp=1;
      if (G_Temp<-1) G_Temp=-1;
           G_Z=(G_Z_Old3+G_Z_Old2+ G_Z_Old1+ G_Temp)/4.0;     
       G_Z_Old3=G_Z_Old2;
       G_Z_Old2=G_Z_Old1;
       G_Z_Old1= G_Temp;


  }
  /*   On calcule l'angle en radians
  * angle = positif si penché en avant, négatif si penché en arrière
  *  -G_X,+G_Z si le CI est horizontal
  *  +G_Z,+G_X si le CI est vertical
  * Angle_Accel = atan2( -G_X, +G_Z );
  */
  Angle_Accel = atan2(+G_Z,+G_X);
  // et enfin, on va positionner l'alarme en cas d'angle hors limite
  if (Angle_Accel>Hors_Limite  ||Angle_Accel<-Hors_Limite ){
      Accel_Alarme=true;
  }else if (Accel_Alarme){             // on ne sort que si l'angle est faible
      if (Angle_Accel<Restart  && Angle_Accel>-Restart)       
         Accel_Alarme=false;
  }

Vous noterez qu'on intègre les lectures (G_X_Old et G_Z_Old) : l'accéléromètre est un composant très bruyant et sensible à la moindre vibration !

Vous noterez aussi le déclenchement d'une alarme si l'angle devient trop important et également le retour en fin d'alarme si l'angle devient très faible -Hors_Limite est fixé à 0.5 degrés-.

Détecteur de distance 

  Pffff le code est trivial puisque les détecteurs donnent une tension qui varie en fonction de la distance. Et comme les détecteurs sont symétriques et "regardent" à 45 degrés, on va dire grossièrement que l'angle varie en fonction de la différence de distance détectée par chaque capteur :

void Lit_Distance() {
   set_adc_channel(D_AVANT);
   delay_us(30);
   Dist_Av = read_adc();

   set_adc_channel(D_ARRIERE);
   delay_us(30);
   Dist_Ar = read_adc();
   Angle_Opto= ((float)(Dist_Ar-Dist_Av-trimSharp))*0.00275;  //résultat en radians
}

Voilà.. on a maintenant les résultats des capteurs gyroscope, accéléromètre et capteurs optique : Tout reste encore à faire !!!

Les trims

Une chose est d'acquérir les infos des capteurs, une autre est de les calibrer ! Mais c'est possible enr éfléchissant un peu :

Tout d'abord, il est "évident" que la vitesse moyenne de rotation détectée par le gyroscope est nulle

Ensuite, on va passer quelques secondes (16 pour être précis) après l'allumage du robot en le tenant bien vertical, ce qui va permettre de calibrer l'accéléromètre et les détecteurs optiques.

La différence entre les 2 points précédents est que la vitesse moyenne du gyro est calculée tout le temps (intégration glissante) alors que les calibrage accéléromètre et opto ne sont faits qu'à l'allumage.

Point besoin d'être parfaitement vertical : on a vu dans le logiciel d'asservissement qu'il existe une formule qui tient compte de la pente éventuelle pour tenir quand même debout : elle corrige également les petits écrts de calibrage.

Le code des trims est identique, aussi je n'en cite qu'un seul : on cumule les valeurs lues dans une table de float. Chaque float cumule une seconde de mesure. Puis, à chaque seconde, on fait le total des N dernières secondes pour en extraire la valeur moyenne glissante qui servira de trim.

Voici le trim de la vitesse du gyro (toujours actif)

/**
*  Calculer le trim de la vitesse du gyro
*
**/
    if (Gyro_comptA==0) Gyro_vit[Gyro_comptB]=0;  // effacer avant première intégration
    Gyro_vit[Gyro_comptB] += Duree_Gyro;          // on intègre
    Gyro_comptA ++;                               // compteur 1 sec                    
    if (Gyro_comptA>49){                          // on a intégré 50 fois 20msec=1sec?
        Gyro_comptA=0;                              // redémarrer le compteur 1sec
        Gyro_comptB ++;                             // incrémenter le compteur d'entrée
        if (Gyro_comptB>15) Gyro_comptB=0;          // modulo 15
    total=0;                                    // calcul de la vitesse ces 15 dernières secondes
        for (i=0;i<16;i++)total +=Gyro_vit[i];      // calcul de la vitesse moyenne
    total = (total>>4);                         // vitesse sur 1 seconde
     /*
     * si on est en phase de calibrage et que le filtre est stable,
     * alors, on va terminer la phase et initialiser l'angle et le bias du gyro
     */
    if(Calibrage){
         Delta_Trim = (float)(RECEIVE) -((float)(total)/50.0) -Gyro_Trim;
         if((Delta_Trim>-1) && (Delta_Trim<1))   Calibrage=false;
    }
    Gyro_Trim= (float)(RECEIVE) -((float)(total)/50.0);    // maj trim de vitesse tout le temps           
 }
/**
* En déduire la vitesse angulaire du gyroscope
*
*la variable Duree_Gyro contient la somme des 2 dernières mesures
**/

     Vitesse_Gyro = ((float)(Duree_Gyro - RECEIVE+ Gyro_Trim))*SENSIBILITE;   // en radians par seconde
 

  Vitesse_Gyro contient donc la vitesse débiaisée... même si le gyroscope dérive, cette vitesse restera vraie.

Les trims de l'accéléromètre et de l'opto sont identiques mais ne sont calculés que tant que Calibrage est vrai... pendant que l'on tient sagement le robot à la verticale (noter que dans ces derniers cas, l'intégration se fait sur 8 secondes et non 16 donc elle se termine toujours avant que calibrage devienne  faux.

Et maintenant ? : Nous avons la vitesse angulaire, l'angle de l'accéléromètre, celui calculé à partir des sharps...Pour être complet et s'amuser sur les combinaisons, il nous manque l'angle calculé à partir de l'intégration de la vitesse du gyro...

L'angle à partir du gyro

 Il s'agit d'intégrer la vitesse angulaire... tout en sachant que le gyro est très bas de gamme et va donc dériver terriblement !

On va donc calculer la dérive de cet angle en prenant comme principe que, lorsque le robot est debout, il est en moyenne vertical.....

 /***************************************
* Angle GYRO
****************************************/
  Angle_Gyro += Vitesse_Gyro*Filtrage_Period;    //intégration pour calculer l'angle si on est pas en alarme

/**
* calculer le trim de l'angle du gyro (on le fait sur 8 secondes)
*
**/
if (Calibrage) Angle_Gyro=0;                     // maintenir angle nul durant le calibrage
if (!Accel_Alarme){                              // s'il n'y a pas d'angle excessif, calculer                
    if (Ang_gyrA==0) Ang_zero_gyr[Ang_gyrB]=0;     // effacer avant première intégration
    Ang_zero_gyr[Ang_gyrB] += Angle_Gyro;          // on intègre
    Ang_gyrA ++;                                   // compteur 1 sec                     
    if (Ang_gyrA>24){                              // on a intégré 25 fois 20msec=1sec?
        Ang_gyrA=0;                                  // redémarrer le compteur 1sec
        Ang_gyrB ++;                                 // incrémenter le compteur d'entrée
        if (Ang_gyrB>15) Ang_gyrB=0;                 // modulo 15
    somme=0;                                     // calcul de l'angle moyen 15 dernières secondes
        for (i=0;i<16;i++)somme +=Ang_zero_gyr[i];   // calcul de l'angle moyen
    Trim_Angle_Gyro= -somme/400.0;               // maj trim  gyro(on divise par 25 fois 16)
  }

}else{                                           // s'il y a angle excessif, reset du trim et du calcul
    for(i=0;i<16;i++) Ang_zero_gyr[i]= 0 ;
  Trim_Angle_Gyro=0;
  Angle_Gyro=Angle_Accel;
}

 

Noter que si le robot est tombé (l'accéléromètre montre un angle excessif, on s'empresse de remettre à zéro les données de calcul.. tout sera redémarré quand l'accéléromètre détectera à nouveau un passage à la verticale

La suite 

  Voilà pour aujourd'hui...

Nous possédons toutes les données traitées :

 - Vitesse angulaire

- Angle donné par l'accéléromètre

- Angle donné par les capteurs optos

- Angle calculé à partir de l'intégration de la vitesse angulaire 

Au prochain numéro, la fin qui comprend le filtrage Kalmal et le choix des données à transmettre à l'asservissement.....

 L'odométrie c'est quoi ?

L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Le terme vient du grec hodos (voyage) et metron (mesure).

Sur le robot Pilou, on dispose de capteurs sur chaque roue. Le processeur chargé de l'odométrie va donc calculer le déplacement et la vitesse de chaque roue.

Voyons comment on fait :

 Le déplacement

 Principe 

Les capteurs utilisé par roue sont deux senseurs situés à 90° au centre desquels tourne une petite roue munie d'aimants... Il y a 3 aimants.

On va donc avoir 6  inversion de polarité sur chaque capteur par tour de moteur donc 12 au total.

Le moteur est doté d'un réducteur 1:30 : on obtiendra donc 360 changements de polarité par tour de roue soit 1 changement par degré.

Les roues font 10cm de diamètre donc : une inversion = 0,87mm

Pour connaitre le sens de rotation, il faut lire les 2 capteurs : le changement de polarité de l'un avec l'état de l'autre nous donne le sens de rotation du moteur... 

 Le logiciel

  Le logiciel doit être suffisamment rapide car il aura a agir 2 fois tous les 0,87mm ... pour un robot qui parcours 3 mètres par seconde, cela fait 7000 fois par seconde donc tous les 140 µ secondes.

Pour ce faire, il existe sur le microprocesseur 4 entrées (RB4 à RB7) qui ont comme propriété de provoquer une interruption dès que l'un d'eux change d'état... On va donc connecter les 4 capteurs à ces broches et attendre les interruptions.

Pour avoir un code rapide comme l'éclair, on utilise la technique suivante :

on crée un octet composé de deux fois les 4 bits  actuels et précédents

On mémorise les 4 bits actuels pour les utiliser comme précédent la prochaine fois

On utilise l'octet pour chercher une valeur dans une table : cette valeur peut être 0 , +1, -1 suivant qu'on doit ne rien faire ou ajouter un ou retrancher un  au déplacement. Il existe une table pour la roue gauche et une table pour la roue droite. Chaquer table fait 256 octets.

Voilà....  le plus fastidieux est de garnir les 2 tables : il faut analyser, roue par roue quelle est la situation actuelle, quelle était la précédente et déduire s'il faut touvher au compteur de distance...

Mais c'est plus rapide que l'éclair (environ 50 instructions machine)

Voici donc le code de l'interrupt

/****************************************
/ Interrupt changement d'état RB4 RB7
****************************************/
// Pour les encodeurs de roues droite et gauche

#int_RB
void RB_isr() {
   codeur_N = input_b() & 0xF0;       // 4 bits poids fort = nouvel état
   codeur_A >>=4;                     // ancien état sur 4 bits poids faible
   codeur_N += codeur_A;              // ou pour faire l'index
   codeur_A = codeur_N & 0xF0;        // mémorisation du nouvel état dans l'ancien
   deplacement_G += dep_G[codeur_N];  // déplacement roue gauche
   deplacement_D += dep_D[codeur_N];  // déplacement roue droite
}

 et les tables utilisées :

signed byte const dep_D[256] =
     {
         0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
         0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
         0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
         0,0,0,0, 0xFF,0xFF,0xFF,0xFF, 1,1,1,1, 0,0,0,0,
        
         1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
         1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
         1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
         1,1,1,1, 0,0,0,0, 0,0,0,0, 0xFF,0xFF,0xFF,0xFF,
        
        
         0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
         0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
         0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
         0xFF,0xFF,0xFF,0xFF, 0,0,0,0, 0,0,0,0, 1,1,1,1,
        
         0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
         0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
         0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
         0,0,0,0, 1,1,1,1, 0xFF,0xFF,0xFF,0xFF, 0,0,0,0,
     };

signed byte const dep_G[256] =
    {
      0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
         0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
         1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
         0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
        
      0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
         0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
         1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
         0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
        
        
      0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
         0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
         1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
         0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
        
      0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0, 0,1,0xFF,0,
         0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1, 0xFF,0,0,1,
         1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF, 1,0,0,0xFF,
         0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0, 0,0xFF,1,0,
       
       
    };

   en résultat, on a deux "long" qui contiennent la distance parcourue par chaque roue : deplacement_G et deplacement_D.

Simple non?

Eh bien pas tout à fait : il faut prévoir le débordement des compteurs au bout d'un moment.... mais ceci n'est pas traité dans ce processeur mais dans le processeur de navigation..... 

La vitesse

  Puisque ce processeur est dédié à l'odométrie, on en profite pour qu'il calcule en permanence la vitesse de chaque roue... en effet, il faut mieux la calculer dans un processeur qui a peu de traitement compliqués et qui assurera un calcul fiable.

Qu'est ce que la vitesse? C'est la distance parcourue par unité de temps... oui mais quelle unité de temps  choisir pour calculer? une fois par heure? trop long, une fois par 10msec? trop court...

Principe

On va mettre en interruption horloge toutes les 10 msec, un programme qui va

• Stocker la différence de déplacement pae rapport au dernier passage dans un buffer "tournant"

• Calculer le déplacement réalisé durant les 50 dernières millisecondes  : ce sera la vitesse en unité de déplacement par 50msec.

On fait donc un calcul toutes les 10 milisecondes qui correspond à la vitesse moyenne des 50 dernières millisecondes 

Le Logiciel

 Voici donc le code du logiciel qui tourne sous interruption horloge 10 msec :

/***************************************
/ Interrupt Timer 0 (toutes les 10msec)
***************************************/
#int_TIMER0
void TIMER0_isr()
 {
       byte ilire;
       byte index;
       byte tempVitesseG;
       byte tempVitesseD;
      
       set_timer0(VAL_TMR0); //période = 10,0352 msec

       // stockage de la vitesse dans le bon slot
       cptTabVitesse++;                              // échantillonnage tics/10,0352msec
       if (cptTabVitesse== NBTABVIT ) cptTabVitesse=0;
       tabVitesseG[cptTabVitesse]= deplacement_G - oldDeplacementG ;   
       tabVitesseD[cptTabVitesse]= deplacement_D - oldDeplacementD ;
       oldDeplacementG = deplacement_G; // mémorisation valeur précédente du déplacement
             oldDeplacementD = deplacement_D;
       // calcul de la vitesse sur les x derniers slots
             cptLireVitesse++;
             if (cptLireVitesse== NBTABVIT ) cptLireVitesse=0;
             tempVitesseG=tempVitesseD=0;
       for (index=0; index<NBSLOTSVITESSE ; index++){
            ilire=cptLireVitesse+index;
            if (ilire>=NBTABVIT) ilire =ilire-NBTABVIT;
            tempVitesseG += tabVitesseG[ilire];
            tempVitesseD += tabVitesseD[ilire];            
       }
       vitesseG= tempVitesseG ;                      // maj vitesse : pas besoin d'inhibit pour la lire
        vitesseD= tempVitesseD ;

  } 

  et les déclarations dans le fichier .h :

// Calcul de la vitesse
#define NBTABVIT 10             // taille totale de la table
byte tabVitesseG[10];            // table des vitesses instantanées
byte tabVitesseD[10];
#define NBSLOTSVITESSE 5         // nombre de slots de 10msec pour calculer la vitesse
byte cptTabVitesse;              // le compteur de remplissage
byte cptLireVitesse;             // le compteur de début de lecture pour le calcul de vitesse
byte  vitesseG;                  // la vitesse lissée sur les NBSLOTSVITESSE dernières 10sec
byte  vitesseD;
byte accelG;                     // l'accélérationgauche
byte accelD;                     // l'accélération roue droite
long oldDeplacementG;            // valeur ancienne du déplacement
long oldDeplacementD;

simple ?  oui .. ici, pas de problèmes

  Fin

Voilà.... il reste au processeur d'odométrie à transmettre ces informations par le bus I2C au processeur d'asservissement quand ce dernier voudra bien venir  les chercher....Il aura alors la dernière valeur toute fraiche du déplacement et de la vitesse

La navigation et l'interface homme machine

Cette troisième carte comporte deux processeurs et est destinée à assurer les entrées sorties avec le maître du robot ainsi qu'à la navigation du robot. Elle va donc recevoir les ordres émis par la télécommande infra rouge (une télécommande récupérée d'un lecteur de dvd), elle va afficher les informations pertinentes sur le petit afficheur situé sur le robot. Elle va enfin utiliser les télémètres infra rouge pour identifier/éviter les obstacles et piloter la carte d'asservissement en conséquence.

Noter que cette carte n'est pas spécifique d'un robot à 2 roues et qu'elle est largement sous-utilisée (on aurait pu utiliser un gps, une boussole, une cartographie....) Mais qui sait si elle n'aura pas des clones sur d'autres robots qui tondent la pelouse par exemple!

Matériel

La carte est très simple et se compose de deux processeurs (toujours des 18F4550 merci l'achat en nombre) pilotés par un quartz à 40Mhz.

L'un des processeurs s'intéresse à l'interface homme machine : on lui connecte une diode IR qui permet de recevoir la télécommande infra rouge et un afficheur LCD 2x16 caractères avec réglage du contraste.

L'autre lit les 6 télémètres de détection des obstacles  - 5 vers l'avant et un vers l'arrière) et pilote la carte d'asservissement via le bus I2C en esclave.

Ces 2 processeurs dialoguent en I2C (le processeur de pilotage est mâitre et le processeur IHM est esclave)

Cette carte étant simple, on a voulu tester 2 points :

• Tous les composants passifs sont en CMS, ce qui en fait une carte très aérée.

• Et surtout, on a essayé une alimentation 5V à découpage :

Alimentation à découpage

L'alimentation est fournie par une batterie LIPO de 4 éléments donc d'environ 15 Volts. Utiliser un circuit régulateur normal  pour délivrer 5 Volts est un gachis en énergie et en thermique. L'alimentation en 5V de la carte d'asservissement exige un radiateur pour 2 malheureux micros... alimenter 2 cartes et 8 télémètres et l'afficheur... aurait exigé une dissipation de plusieurs watts.

Il fallait donc essayer ces circuits intégrés de régulation à découpage. Si la diode schottky est facile à trouver, il a quand même fallu récupérer la self sur une carte électronique en panne.... mais le résultat est positif : mes futurs circuits utiliseront ce mode d'alimentation (on constate évidemment que l'intensité consommée diminue quand on augment la tension cqfd).

Le schéma électrique est le suivant : 

  Toutes les entrées sorties ne sont pas utilisées pour Pilou (les entrées analogique du processeur d'entrée sortie ne sont pas connectées par exemple..)

Voici une photo de la carte équipée et montée sur le robot.

  Le processeur de navigation est situé à droite ; chaque télémètre utilise un connecteur. Le processeur d'entrée sortie est situé au centre et est raccordé à l'écran afficheur (connecteurs à gauche) ainsi qu'au led d'alarme batterie etdiode de réception de télécommande (connecteurs au centre). Sur la gauche, on voit l'alimentation à découpage avec le circuit intégré sans radiateur ,  la self, les condensateurs et le fusible.

Les 4 LEDS implantées sur la carte servent au deboggage et à aider dans l'utilisation du robot (par exemple, l'un des LEDS est utilisé comme acquit de réception d'une commande infra rouge, ce qui s'avère indispensable à l'utilisation). 

Noter que cette carte alimente aussi, via le connecteur I2C, la carte de détection d'attitude.

  Logiciel

Chaque processeur est dédié à des activités diférentes.

 Processeur de navigation

Le logiciel de ce processeur est des plus simple puisque son sul rôle est de commander la carte des moteurs (vitesse et direction) et d'éviter les obstacles en mode automatique.

Il gère les modes de fonctionnement du robot dont 2 ont été développés :

• Le mode manuel qui reçoit les commandes de vitesse et de direction du processeur d'interface et les  exécute

• Le mode évitement qui reçoit les commandes de vitesse (en marche avant seulement) du processeur d'interface et qui réalise un parcours aléatoire en évitant les obstacle.

Evitement

 Chaque télémètre est interrogé toutes les 40 msec.

• Le  logiciel en fait  une "carte" représentant l'état de l'éspace devant lui sur environ 100°.

• En fonction de la distance d'un obstacle, il va éventuellement diminuer la vitesse voire passer en marche arrière

• En fonction de la "carte ", il va choisir la moins mauvaise des directions à prendre pour éviter ou raser l'obstacle.

 Cette dernière fonction est réalisée par une table (indexée par la valeur de la carte) qui donne la valeur du volant à appliquer. Le robot ne s'arrête donc pas pour tourner mais va moduler la position du volant et de la vitesse en fonction des obstacles et de leur distance. Cet algorithme donne un résultat assez réaliste et le robot a tendance à frôler l'obstacle plutôt que d'opérer des virages brusques.

 Arrêt / marche

On rappelle que le robot est asservi en vitesse lorsque la consigne n'est pas nulle et en position lorsqu'il est à l'arrêt.

De même, un angle excessif va couper les moteurs 

Processeur d'interface homme/machine 

Le logiciel est composé de plusieurs modules :

Réception de télécommande infra rouge

Le traitement est fait en interrupt.. La télécommande JVC utilisée a un codage IR particulier (non RC5)  qu'il a fallu analyser à l'oscilloscope et coder.... En résultat, la routine d'interruption donne à la tâche de base l'identité de la touche  de la télécommande qui a été activée. Les temporisations de répétition, lorsqu'une touche est enfoncée en permanence, sont également traitée en interruption ; chaque touche peut avoir une temporisation de répétition spécifique.

Affichage LCD

Les routines d'affichage tournent en tâche de base et sont de 2 niveaux

• Les fonctions élémentaires (effacement, écriture d'un caractère, gestion du curseur....) sont spécifiques de l'afficheur et exigent des temporisations spécifiques

• Les fonctions de messages sont de niveau haut et permettent la composition des éléments à afficher.

Suivant les modes d'entrée sortie (état normal, états de modifications de paramètres, ...), l'afficheur ne visualise pas les mêmes informations...

En résumé, on affiche :

- La tension batterie

- Le compteur cumul du temps de fonctionnement

- Le compteur "journalier" de cumul (utile pour gérer la temps depuis la dernière charge batterie)

ces deux compteurs sont conservés en flash

- Le mode de fonctionnement (manuel ou automatique)

- Le fonctionnement normal ou angle excessif

- La valeur d'un paramètre

- Les confirmations d'activation de valeur de paramètre ou de sauvegarde en flash ou de retour aux valeurs usine

Conclusion

Ces logiciels sont simples et ce sont ces derniers qui doivent évoluer si on désire faire faire au robot des évolutions plus complexes : on garde les processeurs d'attitude et de contrôle des moteurs inchangés donc sans devoir tout retester... notamment le temps réel est les interrupts...

Par exemple, on peut doter le processeur de navigation d'une boussole électronique, d'une mémorisation du parcours, d'une cartographie  etc etc  sans trop de complexité . Mais là de pose la question de la pertinence des fonctions sur un robot équilibriste.... ne vaut il pas mieux réaliser ces fonctions de parcours sur un classique robot à 4 roues ?

Carte attitude

Principe 

C'est une nouvelle aventure !  Cette carte doit fournir à la carte  moteur l'angle d'attitude du robot et  sa vitesse.

Pour expérimenter plusieurs voies, on a disposé 3 capteurs

• Un accéléromètre 2 axes qui permet donc de connaitre l'angle de 0 à 360°

• Un gyroscope "bas de gamme" et "pas cher" utilisé sur les modèles réduits d'hélicoptère. Il donne la vitesse angulaire

• Un système à 2 télémètres mesurant à 45° la distance au sol : une fois étalonné, on peut en déduire en soustrayant les distances et avec une formule trigo idoine l'angle du robot.

 On peut donc, en s'aidant d'un filtre de Kamann, calculer l'attitude par

• L'accéléromètre+gyroscope

• Les télémètres+gyroscope

• Le gyroscope seulement

Mais, me direz vous : le gyroscope dérive donc l'angle va devenir faux... comment donc ne se servir que du gyroscope ?  Patience, on aborde plus loin cette idée géniale(!)

 Matériel

Pour faire un peu moderne et parce qu'elle est simple, cette carte utilise, modérément, les résistances et condensateurs CMS.

Pour offrir un maximum de puissance La fréquence du quartz qui pilote le 18F4550 est de 40Mhz 

Le schéma électrique est le suivant : 

  Le microprocesseur est un 18F4550 (toujours le même suite à un achat groupé!) piloté par un quartz à 40Mhz.

Le gyroscope est commandé en pwm et sort du pwm respectivement aux broches rc1 et rc2. 

L'accéléromètre est un ADXL202 monté sur un boitier 14 broches (d'où le brochage exotique). Ce boitier est perpendiculaire à la carte pour pouvoir mesurer correctement les accélérations  en X et Y. Les sorties pwm X et Y sont connectées aux broches  RB4 et RB5

Les 2 télémètres sharp GP2D12 sont connectés aux ports analogiques via le connecteur X3 

Enfin, le bus esclave I2C est connectévia X1  aux broches RB0 et RB1

Les 4 leds servent à afficher le signe de l'angle et celui de la vitesse angulaire

Carte simple donc et qui ne demande pas d'explications compliquées. 

Voici donc une photo de cette carte "in situ" comme on dit en Anglais !

  On y retrouve facilement le gyroscope, l'accéléromètre sous ce dernier, le quartz et le microprocesseur ainsi que les leds et connecteurs. Le bouton poussoir est inutilisé. Les résistance et autres condensateurs de découplage sont monté en CMS coté piste.

Noter enfin les deux télémètres à 45° situés au dessus de la carte. 

Logiciel

Un logiciel qui a demandé de la réflexion que celui de la carte d'attitude ! Elle permet de multiples possibilités dont plusieurs restent encore à expérimenter.

 Gyro

Tout d'abord, il fallait piloter le gyroscope en lui envoyant du pwm et surtout en mesurant le pwm reçu pour en déduire une vitesse angulaire.

CCP2 est utilisé en mode "CCP_COMPARE_INT_AND_TOGGLE" avec interrupt pour piloter avec précision les durées ON et OFF du pwm appliqué au gyro ; à chaque interrupt, on charge la durée on ou off suivant l'état de sortie. On envoie "1" durant 1 msec et "0" durant 9 msec.

CCP1 est utilisé en alternativement en mode CCP_CAPTURE_RE  et CCP_CAPTURE_FE pour avoir une interruption à chaque front montant et descendant (et leur datation précise) de la sortie du gyroscope.

On peut donc calculer le rapport cyclique en réception.Enfin, on fait deux mesures sonsécutives pour moyenner cette valeur et la rendre disponible tous les 20msec.

Si une interruption gènante (typiquement l'interrupt I2C esclave) intervient durant une mesure, cette dernière est invalidée.

Pour éviter de régler le gyroscope, on réalise également une correction faite à partir d'une moyenne glissante de toutes les valeurs reçues durant 16 secondes : cette moyenne doit être nulle. On corrige donc la vitesse angulaire en permanence par ce trim glissant : la moyenne de la vitesse angulaire reste nulle. Noter qu'il n'y a jamais eu besoin de retoucher les réglages du gyroscope depuis la construction du robot !

Il va de soi qu'il faut donc, à la mise sous tension, attendre 16 secondes pour obtenir la valeur du premier trim glissant : cette phase est appelée "calibration" et le robot ne démarre pas ses moteurs tant qu'elle n'est pas terminée.

En intégrant cette vitesse, on a l'angle. 

Accéléromètre

 L'accéléromètre travaille également sur la réception d'un rapport cyclique (Un pour l'axe des X et un pour l'axe des Y).

Ces rapports sont obtenu par une boucle tournant en niveau de base lancée immédiatement après l'acquisition d'une valeur du gyro  et mesurant p ar une boucle les durées ON et OFF de chaque axe X et Y de l'accéléromètre.

Si une interruption survient pendant une mesure, celle ci est invalidée.

en combinant X et Y, on obtient l'angle de l'attitude, de 0 à 360°, mesuré par l'accéléromètre.

 Télémètres infra rouge

L'utilisation des télémètres infra rouge Sharp est des plus simples en utilisant les entrées analogiques... On recoit une valeur dépendant de la distance entre chaque télémètre et le sol. L'angle en radian ,tant que cet angle est faible ce qui est le cas, est proportionnel à la différence entre la distance mesurée par le télémètre à l'avant et celle mesurée par le télémètre à l'arrière.

 Récapitulatif des informations

Voilà pour l'acquisition des informations : on possède donc, toutes les 20 msec

- Une vitesse angulaire (dont la moyenne glissante sur 126 sec est nulle) 

- Un angle calculé en intégrant la vitesse angulaire ci dessus.

- L'angle mesuré par l'accéléromètre

- L'angle mesuré par les télémètres.

A partir de là, on peut essayer de multiples combinaisons. La première, décrite sur des sites web, consiste à utilise un filtre de Kalman avec accéléromètre et gyroscope pour corriger la dérive du gyroscope.

 Filtre de Kalman

 On injecte toutes les 20 msec l'angle gyro et l'angle de l'accéléromètre pour obtenir en sortie l'angle estimé. Le fichier source C utilisé est dérivé de rotomotion_tilt.c disponible sur le web.

Le filtre tourne donc toutes les 20msec. Les informations vitesse et vitesse angulaire sont transmises (demandées par) à la carte contrôle des moteurs toutes les 20 msec. 

  Résultats et évolutions

Le réglage glissant du zéro de la vitesse angulaire du gyroscope étant très efficace, on a testé 3 configurations qui ont toutes comme points communs

• d'utiliser la vitesse angulaire du gyroscope comme l'information vitesse angulaire émise vers la cart moteur

• d'utiliser l'intégration de cette vitesse comme entrée du filtre de kalman

Les 2 premières configuration utilisent respectivement l'information angle issue de l'accéléromètre et des télémètres comme seconde entrée du filtre de Kalman.

Configurations traditionnelles 

Les deux configurations donnent satisfaction. Les télémètres sont particulièrement efficaces et stables.. ils ont 2 "petits" problèmes : Le robot bascule lorsqu'on approche la point du pied dans le faisceau d'un télémètre et le fonctionnement sur un sol en plein soleil n'est pas satisfaisant (le télémètre est aveuglé) 

Noter que, pour ces deux configurations,  le trim de l'angle est calculé durant la phase de calibration (le robot doit être maintenu approximativement en équilibre durant cette calibration).

Noter enfin que on utilise l'accéléromètre pour détecter les angles hors tolérance et envoyer ainsi une information permettant de couper les moteurs

  Nouvelles configurations

Mais La troisième configuration marche également et est sans nul doute plus prometteuse puisqu'on n'utilise que le gyroscope même s'il a une dérive importante . -pour être exact, l'accéléromètre est encore utilisé comme détecteur d'angle hors tolérance-- En voici en gros le principe :

"Si le robot tient en équilibre, c'est qu'il est en moyenne vertical "

  C'est déjà un principe similaire qui est utilisé pour faire un ajustement dynamique de la vitesse angulaire.

La réalisation de base consiste donc à corriger l'angle brut issu de l'intégration de la vitesse angulaire du gyro par une moyenne glissante de ce dernier (faite ici sur 16 secondes) : le robot marche parfaitement

Mais on peut aussi explorer de ne rien corriger en supposant que le lissage de la vitesse angulaire est bon (vitesse angulaire moyenne nulle)

On peut aussi, lors de la phase de calibrage, calculer la dérive angulaire et appliquer la correction pour la suite : il ne faut pas oublier que l'asservissement du moteur est capable de corriger une petite dérive de la verticale

Le but serait de compléter la phrase ci dessus : en effet, le problème qui se pose est une mauvaise verticalité quand le robot est tenu manuellement; la verticalité de vient fausse et lacher le robot va provoquer sa chute... 

Mais  il est possible de faire parfaitement avec le seul gyroscope même s'il a une dérive importante et ce sera le but des essais actuels. 

Aujourd'hui

Pour faire propre et préparer les expérimentations, le logiciel a été modifié et on calcule un trim (une valeur moyenne glissante des X dernières secondes) de chaque information .

- vitesse angulaire gyro

- angle par intégration de la vitesse angulaire gyro

- angle par accéléromètre

- angle par optique

Ceci a permis de faire un graphique montrant le résultat de chaque information pendant le balancement du robot et donc de comparer la qualité des résultats. 

Le gyro dérive beaucoup, ce qui oblige à calculer son trim sur un lissage de 8 secondes (et non 16 secondes comme pour les autres variables)

Mais le robot fonctionne très bien ainsi. Exit le filtre de Kalman, exit le traitement par opto, exit les calculs de l'accéléromètre.

Il a fallut soigner la procédure de démarrage et de remise à la verticale après une chute :

1) l'accéléromètre est uniquement utilisé pour détecter un angle anormal qui nécessite l'arrêt des moteurs

2) à l'initialisation, on est supposé tenir le robot à la verticale, ce qui permet de calibrer non seulement la vitesse détectée  par le gyro mais aussi le zéro de l'accéléromètre et l'angle calculé à partir du gyro.

3) en cas de chute : l'accéléromètre stoppe les moteurs et on réinitialise l'intégration de l'angle calculé par le gyro.

4) pour redémarrer le robot, il suffit ensuite de le positionner à la verticale (à un ou deux degrés près) : il reprend son balancement.

Conclusions

Un gyro même de faible prix suffit pour connaitre l'attitude du robot. L'accéléromètre est trop bruyant ( sauf si on isole drastiquement ce dernier de la structure en aluminium qui propage très bien les vibrations des moteurs !)  et  ne sert qu'à détecter un angle excessif et un retout à la verticale

Il faut une bonne procédure de démarrage du balacement

Pas besoin de filtres passe-haut/passe-bas ou kalman

Voir les détails du programme dans l'article dédié