Club robotique de Sophia-Antipolis

Accueil > Projets et études > Nos robots > Robots du club > Robots spéciaux > Nono, le robot d’animation pédagogique (2012) > Programmes de tests de Nono

Programmes de tests de Nono

lundi 25 juin 2012, par Julien H.

Notre robot Nono dispose de plusieurs systèmes, il est donc important de pouvoir tous les tester régulièrement :
- il peut tourner la tête
- il peut éclairer différentes parties de sa tête
- il peut se dandiner sur son socle
- il peut mesurer une distance devant lui (télémètre infra-rouge)
- il peut détecter une présence à gauche ou à droite (capteurs infra-rouge)
- il peut cracher de l’eau (mécanique absente, non câblé)
- il peut détecter 3 pressions sur ses doigts (non câblé)

 Liste des ports utilisés

PatteRôle
D0liaison série
D1liaison série
D2contact du cou à gauche (interruption 0)
D3contact du cou à droite (interruption 1)
D4direction du moteur de socle
D5vitesse du moteur de socle
D6vitesse du moteur du cou
D7direction du moteur du cou
D8
D9bouche
D10bouche
D11bouche
D12bouche
D13bouche
14capteur TOR oreille
15capteur TOR oreille
16lumière
17lumière
18lumière
19lumière

Note : les numéros 14 à 19 correspondent aux pattes A0 à A5 quand on les utilise en numérique, pour allumer des leds ou pour des capteurs tout ou rien (TOR).

 Tests sur Arduino

Retrouvez les codes sources sur GitHub. On peut ainsi charger la carte Romeo qui équipe la tête de Nono avec l’un des codes pour Arduino qui effectue des actions programmées. Pour le contrôle depuis la carte Raspberry Pi, on peut utiliser Firmata ou un autre programme générique de pilotage par commandes séries.

Les contacts par micro-switch

Le cou est équipé de deux contacts. Voici un premier code pour tester facilement un des contacts, connecté sur l’entrée numérique n°2.

Le contact est fermé au repos (NC sur le micro-switch) et se coupe lorsqu’il y a contact. L’entrée doit donc être câblée avec une résistance de pull-down (ou push-down) soit un signal bas (la masse). On peut utiliser la résistance interne mais personnellement ça n’a jamais marché : j’utilise une résistance de 10k.

void setup()
{
  Serial.begin(9600);
  Serial.println("Nono test contacts");
 
  pinMode(2,INPUT);
  digitalWrite(2,LOW);
   
  Serial.println("Pret...");
}
void loop()
{
  Serial.println(digitalRead(2));
  delay(200);
 
}

Ce qui affiche des 1 en répétition, sauf lorsque le contact est appuyé sur le microrupteur.

Voici le même test avec une interruption :

void setup()
{
  Serial.begin(9600);
  Serial.println("Nono test contacts");
 
  pinMode(2,INPUT);
  digitalWrite(2,LOW);
 
  attachInterrupt(0,contactGauche,CHANGE);
   
  Serial.println("Pret...");
}
void contactGauche()
{
  if (digitalRead(2) == HIGH) {
     Serial.println("ouvert");
  } else {
     Serial.println("ferme");
  }  
  delay(200);
}
void loop()
{
 
}

Rotation de la tête

Le moteur du cou est le moteur 2. Il utilise deux pattes logiques : 7 pour la direction et 6 pour la vitesse (sachant qu’on l’utilisera en tout ou rien).

Voici le code qui met en mouvement la tête dans le sens horaire (de gauche à droite), change le sens quand le contact gauche (tête visant à droite) est déclenché, et change le sens quand le contact droit (tête visant à gauche) est déclenché.

void setup()
{
  Serial.begin(9600);
  Serial.println("Nono test contacts");
  // les contacts
  pinMode(2,INPUT);
  digitalWrite(2,LOW);
  pinMode(3,INPUT);
  digitalWrite(3,LOW);
  attachInterrupt(0,contactGauche,CHANGE);
  attachInterrupt(1,contactDroite,CHANGE);
  // le moteur du cou
  pinMode(7,OUTPUT); // la direction
  pinMode(6,OUTPUT); // la vitesse
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH); // on débute dans le sens horaire
  Serial.println("Pret...");
}
void contactDroite()
{
  if (digitalRead(3) == HIGH) {
    Serial.println("ouvert");
  }
  else {
    Serial.println("ferme");
    // change la direction --> horaire
    digitalWrite(7,HIGH);
  }  
  delay(200);
}
void contactGauche()
{
  if (digitalRead(2) == HIGH) {
    Serial.println("ouvert");
  }
  else {
    Serial.println("ferme");
    // change la direction --> antihoraire
    digitalWrite(7,LOW);
  }  
  delay(200);
}
void loop()
{
}

Faire bouger le socle

Le moteur connecté sur le premier contrôleur de moteur à courant continu de la carte Arduino Romeo est situé dans le socle de Nono, faisant tourner une tige qui peut varier de hauteur en fonction de deux obstacles situés de part et d’autre. Cette tige est tenue par une vis (attention, ne la vissez pas complètement, il faut laisser du jeu !!) pour ne pas tomber. Lorsqu’elle passe sur les obstacles, le socle de Nono bouge et les ressorts accentuent ce tressautement. Une astuce de notre ami Jean-Pierre, le papa de Nono.

Ce code très simple fait tourner le moteur dans un sens puis dans l’autre.

// Test du socle bougeant de Nono
void setup()
{
   // rien de spécial ici, tout est géré dans la boucle
}
void loop()
{
    // changer le sens de rotation du moteur
    digitalWrite(4,LOW);
    // mettre en marche à vitesse maximum (il y a une démultiplication)
    digitalWrite(5,HIGH);
    delay(1000);
    // arrêter
    digitalWrite(5,LOW);
    delay(1000);
   
    // changer le sens
    digitalWrite(4,HIGH);
    // mettre en marche
    digitalWrite(5,HIGH);
    delay(1000);
    // arrêter
    digitalWrite(5,LOW);
    delay(1000);
}

Tourner la tête vers le public

Cette fois-ci, on utilise les capteurs.

volatile boolean troploin = false;
boolean inactif = true;
long last_actif;
void setup()
{
  Serial.begin(115200);
  Serial.println("Robot Nono pret !");
 
  // led 13 à 9, la bouche
  pinMode(13,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(9,OUTPUT);
  // les contacts
  pinMode(2,INPUT);
  pinMode(3,INPUT);
  attachInterrupt(1,contactGauche,CHANGE);
  attachInterrupt(0,contactDroite,CHANGE);
  // le moteur du cou
  pinMode(7,OUTPUT); // la direction
  pinMode(6,OUTPUT); // la vitesse
  digitalWrite(6,HIGH);
  digitalWrite(7,HIGH); // on débute dans le sens horaire
  // les capteurs des oreilles
  pinMode(14,INPUT);
  pinMode(15,INPUT);
  // le moteur du bas
  pinMode(4,OUTPUT); // la direction
  pinMode(5,OUTPUT); // la vitesse
}
void contactDroite()
{
  if (digitalRead(2) == HIGH) {
    digitalWrite(13,LOW);
  }
  else {
    digitalWrite(13,HIGH);
    troploin = true;
  }  
}
void contactGauche()
{
  if (digitalRead(3) == HIGH) {
    digitalWrite(13,LOW);
  }
  else {
    digitalWrite(13,HIGH);
    troploin = true;
  }
}
void loop()
{
  // si on vient d'etre en blocage
  if (troploin) {
    if (digitalRead(3) == HIGH) {
        Serial.println("Tourner à gauche");
      // change la direction --> antihoraire
      digitalWrite(7,LOW);
    }
    if (digitalRead(2) == HIGH) {
        Serial.println("Tourner à droite");
      // change la direction --> horaire
      digitalWrite(7,HIGH);
    }
    digitalWrite(6,HIGH);
    delay(6000);
    digitalWrite(6,LOW);
    troploin = false;
  }
  // si les deux capteurs captent : on s'arrete
  if (digitalRead(14) == digitalRead(15))
  {
    digitalWrite(6,LOW);
    digitalWrite(9,LOW);
    digitalWrite(17,LOW);
    digitalWrite(16,LOW);
  }
  else {
    inactif = false;
    // il faut tourner
    digitalWrite(6,HIGH);
    digitalWrite(9,HIGH);
    // on détermine le sens
    if (digitalRead(15) == HIGH) {
      digitalWrite(16,HIGH);
      digitalWrite(7,HIGH);
    }
    else {
      digitalWrite(17,HIGH);
      digitalWrite(7,LOW);
    }
  }
  // détecte une inactivité
  if (! inactif) {
    last_actif = millis();
  }
  else {
    // que fait-on quand on est inactif ? on allume les leds
    if (millis()%2000<1000)
    {
      lumiere_off();
      bouche_on();
      digitalWrite(6,HIGH);
      digitalWrite(7,HIGH); // on tourne dans le sens horaire
    }
    else {
      bouche_off();
      lumiere_on();
      digitalWrite(6,HIGH);
      digitalWrite(7,LOW); // on tourne dans le sens antihoraire
    }
  }
  if (millis()-last_actif > 5000) {
    inactif = true;    
  }
}
void lumiere_on()
{
  digitalWrite(16,HIGH);
  digitalWrite(17,HIGH);
  digitalWrite(18,HIGH);
  digitalWrite(19,HIGH);
}
void lumiere_off()
{
  digitalWrite(16,LOW);
  digitalWrite(17,LOW);
  digitalWrite(18,LOW);
  digitalWrite(19,LOW);
}
void bouche_off()
{
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
  digitalWrite(11,LOW);
  digitalWrite(12,LOW);
  digitalWrite(13,LOW);
}
void bouche_on()
{
  digitalWrite(9,HIGH);
  digitalWrite(10,HIGH);
  digitalWrite(11,HIGH);
  digitalWrite(12,HIGH);
  digitalWrite(13,HIGH);  
}

Et voilà, à vous de continuer ces petits tests !!

Un message, un commentaire ?

modération a priori

Attention, votre message n’apparaîtra qu’après avoir été relu et approuvé.

Qui êtes-vous ?

Pour afficher votre trombine avec votre message, enregistrez-la d’abord sur gravatar.com (gratuit et indolore) et n’oubliez pas d’indiquer votre adresse e-mail ici.

Ajoutez votre commentaire ici
  • Ce formulaire accepte les raccourcis SPIP [->url] {{gras}} {italique} <quote> <code> et le code HTML <q> <del> <ins>. Pour créer des paragraphes, laissez simplement des lignes vides.

Ajouter un document