Robot : Hector

De troyesGEII
Aller à : navigation, rechercher


Présentation du projet

Préambule du concours inter IUT

Le concours robotique des IUT GEII est une rencontre ouverte aux étudiants de tous les départements GEII de France. Son objectif est triple. La préparation du robot pour la coupe est d’abord un élément de motivation pour les étudiants des IUT GEII. Elle leur permet de mettre en oeuvre concrètement les compétences techniques enseignées en GEII et peut servir de support pour les enseignements. D’autre part, par l’image positive qu’il véhicule, cet événement est destiné à promouvoir les IUT GEII en particulier et les études scientifiques et techniques en général. Enfin, la rencontre est également un événement fort d’échange et de convivialité entre les étudiants et les enseignants. Les deux doivent garder à l’esprit que ces objectifs sont prioritaires sur la victoire du robot représentant son IUT. Le présent règlement précise les conditions de participation à la coupe de robotique des départements GEII. Sa définition poursuit les objectifs suivants:

 • Permettre au plus grand nombre de participer, même avec un robot simple, 
 • Faire du spectacle avec les robots les plus performants, 
 • Favoriser le fair-play et la convivialité lors de la rencontre,
 • Simplifier l’arbitrage des matchs pour le jury,
 • Assurer l’équité entre les différentes équipes,
 • Limiter les collisions entre robots.

La mission du robot et l'arène

Le projet que nous, étudiants en GEII, devons réaliser est un robot qui serait capable de relever le challenge de robotique inter IUT. Le robot doit rouler sur une piste carrée de dimension 8 m x 8 m recouverte de moquette bleue marine sur laquelle sont placés des obstacles de 15 cm de haut. L’objectif est de traverser le plus rapidement possible la piste d’un coin à son opposé.

Ci-dessous, vous pouvez voir 2 exemples d'arènes avec ses obstacles, les zones de départs et d'arrivée. Ces obstacles sont positionnés de façon symétrique de sorte que le parcours soit équivalent pour les quatre robots. Les obstacles ont des bords droits et perpendiculaires au sol. Chaque obstacle est peint d’une couleur unie mais les différents obstacles peuvent avoir des couleurs différentes. Il existe toujours au moins deux chemins de 40 cm de large permettant de rejoindre un coin à un autre. Les obstacles doivent être contournés. Ils ne doivent être ni déplacés ni altérés. Dans cet esprit, un robot qui percute ou pousse de façon systématique le même obstacle sera considéré comme n’étant jamais arrivé. La position des obstacles n’est pas a priori connue et peut changer à n’importe quel moment. Néanmoins, pour permettre à des robots simples de participer, la position des obstacles lors des premières rencontres en phase de qualification sera très similaire, voire identique, à celle présentée sur la figure 1. Au cours de l’évolution de la compétition, la configuration des obstacles évoluera pour devenir plus difficile à traverser, comme dans l’exemple de la figure 2 et les modifications pourront également être plus fréquentes.


Arene2.png Arene.png

Cahier des charges

Lors de la construction du robot nous allons devoir respecter certaines contraintes qui sont les suivantes:

 • Le robot sera construit a partir d'un kit imposé par le comité d'organisation, comprenant le châssis, les moteurs, les roues, les engrenages et la batterie,
 • Le design est libre et donnera lieu à un prix du design qui sera indépendant de la course,
 • Un arrêt d'urgence doit être créé et doit être opérationnel, facilement accessible et impérativement couper la partie puissance,
• Une prise JACK doit obligatoirement être utilisée : au top départ, un étudiant de l'équipe le retire, ce qui va permettre au robot de démarrer,
 • A l'exception du tube destiné à porter un ballon de couleur qui indique vers quel coin le robot doit se rendre, le robot ne doit pas dépasser
les dimensions maximales suivantes : hauteur 30 cm, largeur 30 cm et longueur 40 cm. Le dispositif destiné à faire éclater le ballon ne peut sortir
du gabarit que dans la zone d'arrivée,
 • Chaque coin est identifié au moyen d'une couleur. Chaque robot en course porte un ballon de la même couleur que son coin d'arrivé. Le ballon est
fourni. Il mesure au moins 10 cm de diamètre lorsqu'il est donné à l'étudiant qui a en charge de démarrer le robot. Le robot doit être doté d'un tube
vertical dont l'extrémité haute est situé entre 30 et 31 cm au dessus de la moquette pour servir de support au ballon.
 • Lorsque le robot est arrivé à sa zone d'arrivée, il doit s'arrêter puis, une fois totalement arrêté faire éclater le ballon. C'est à l'instant précis
où, alors qu'il est arrêter au dessus de sa zone d'arrivée et que son ballon éclate que l'arrivée est considéré comme validée. Un robot qui éclate
son ballon ou qui déploie le système destiné à crever son ballon sans être arrivé ou avant d'être complètement immobile ne marquera aucun point quelque
soit son classement d'arrivé.

Dimensions.png

Détection d'obstacle

Capteur à ultrason : HC-SR04

Description du composant

Hcsr04.jpg

Ce capteur est composé de 4 pins : - VCC, 5V supply, - Trigger, Trigger pulse input, - Echo, Echo pulse output, - GND, 0V ground.

Ce module permet d’évaluer les distances entre un objet mobile et les obstacles rencontrés. Il suffit d'envoyer une impulsion de 10 µs en entrée et le capteur renvoie une largeur d'impulsion proportionnelle à la distance. Alimentation: 5 Vcc Consommation: 15 mA Fréquence: 40 kHz Portée: de 2 cm à 4 m Déclenchement: impulsion TTL positive de 10µs Signal écho: impulsion positive TTL proportionnelle à la distance. Calcul: distance (cm) = impulsion (µs) / 58 Dimensions: 45 x 21 x 18 mm

Un premier programme

Programme HC-SR04

int trig = 10;
int echo = 9;
long lecture_echo;
long cm;

void setup()
{
  pinMode(trig, OUTPUT);
  digitalWrite(trig, LOW);
  pinMode(echo, INPUT);
  Serial.begin(9600);
}

void loop()
{
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  lecture_echo = pulseIn(echo, HIGH);
  cm = lecture_echo / 58;
  Serial.print("Distancem : ");
  Serial.println(cm);
  delay(1000);

}

Cependant, afin d'optimiser notre robot, il va falloir qu'il puisse rouler tout en émettant de ondes. Nous allons donc utiliser le TIMER1 et ses interruptions.

TIMER1

AVRMega328 Timer1.png


Pour paramétrer notre timer, il faut choisir la valeur du prédiviseur ainsi que le nombre de fois que le timer s'incrémentera avant de reset comme le décrit le chronogramme suivant :


ChronogrammeTimer.png

(TP info S2)

Voici le programme afin d'utiliser le TIMER1

TIMER_1

//this code will enable all three arduino timer interrupts.
//timer0 will interrupt at 2kHz
//timer1 will interrupt at 1Hz
//timer2 will interrupt at 8kHz

#include <util/delay.h>
//storage variables
boolean toggle1 = 0;
volatile unsigned long t;
unsigned long tfin, tdebut, d;
volatile boolean recpt;
volatile boolean newmesure = false;


void setup() {
  Serial.begin(57600);
  //set pins as outputs
  pinMode(10, OUTPUT);

  cli();//stop interrupts

  //set timer1 interrupt at 10^5Hz
  TCCR1A = 0  ;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  //  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 20000;// = (16*10^6) / ((40546*8) - 1) (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS11 bit for 8 prescaler
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  //configuration interruption
  EICRA |= (1 << ISC00);
  EIMSK |= (1 << INT0);

  sei();//allow interrupts
}//end setup

ISR(TIMER1_COMPA_vect)
{
  PORTB |= (1 << PB4);
  _delay_us(60);
  PORTB &= ~(1 << PB4);
}
  ISR(INT0_vect)
  {
    if (recpt == false)
    {
      tdebut = micros();
      recpt = true;
    }
    else
    {
      tfin = micros();
      t = tfin - tdebut;
      recpt = false;
      newmesure = true;
    }
  }

  void loop() {
    //do other things here
    if (newmesure == true)
    {
      d = t / 58;
      Serial.print("Duree : ");
      Serial.print(t, DEC);
      Serial.println(" us.");
      
      Serial.print("Distance : ");
      Serial.print(d, DEC);
      Serial.println(" cm.");
      
      Serial.println("_");

      delay(1000);
      newmesure = false;
    }
  }

On a alors un capteur qui répond bien et prêt à être tester sur notre robot, cependant le TIMER1 a un influence sur la patte 11 de l'arduino (en PWM), utilisé par l'un de moteurs, la rendant inutilisable. On utilisera donc le TIMER2 qui ne posera aucun problème.

TIMER2

AVR Timer2 Comp.png


Il suffira de modifier quelques parties du TIMER1, on obtient alors ceci :

TIMER_2

#include <util/delay.h>
boolean toggle1 = 0;
volatile unsigned long t;
unsigned long tfin, tdebut, d;
volatile boolean recpt;
volatile boolean newmesure = false;


void setup() {
  Serial.begin(115200);

 //set pins as outputs
  pinMode(10, OUTPUT);

  cli();//stop interrupts

  TCCR2A = 0;
  TCCR2B = 0;
  OCR2A = 156;
  // turn on CTC mode
  TCCR2B |= (1 << WGM12);

  TCCR2B |= (1 << CS11);
  TCCR2B |= (1 << CS10);
  TCCR2B |= (1 << CS12);

  TIMSK2 |= (1 << OCIE1A);

  //configuration interruption
  EICRA |= (1 << ISC00);
  EIMSK |= (1 << INT0));

  sei();//allow interrupts
}//end setup

ISR(TIMER2_COMPA_vect)
{
  static boolean start = true;

  if (start == true)
  {
    PORTB |= 1 << PB4;
    TCCR2B &= ~((1 << CS12) | (1 << CS10));
    OCR2A = 150;
  }
  else
  {
    PORTB &= ~(1 << PB4);
    recpt = false;
    TCCR2B |= ((1 << CS12) | (1 << CS10));
    OCR2A = 30;
  }
  start = !start;
}
ISR(INT0_vect)
{
  if (recpt == false)
  {
    tdebut = micros();
    recpt = true;
  }
  else
  {
    tfin = micros();
    t = tfin - tdebut;
    recpt = false;
    newmesure = true;
  }
}
void loop() {
  //do other things here
  static unsigned long dateAffichage = 0, dateActuelle;
  dateActuelle = millis();
  if (newmesure == true)
  {
    t = tfin - tdebut;
    d = t / 58;

    if ((dateActuelle - dateAffichage) > 200)
    {
      dateAffichage = dateActuelle;
      Serial.print("Duree : ");
      Serial.print(t, DEC);
      Serial.println(" us.");

      Serial.print("Distance : ");
      Serial.print(d, DEC);
      Serial.println(" cm.");
      Serial.println(' ');

    }
    newmesure = false;
  }
}

Des améliorations ont été effectuées afin d'obtenir un robot plus réactif lors de la rencontre d'un obstacle.

Test

Lorsqu'on implémente notre TIMER au programme commandant les roues, notre robot s'arrête comme prévu lorsqu'il détecte un obstacle.

void loop() du programme testé

void loop()
{
  Capteur();
  if (d < 30)
  {
    stopBoth();
  }
  else
  {
    driveArdumoto(MOTOR_A,CCW,100);
    driveArdumoto(MOTOR_B,CW,100);
  }
}

Nous sommes alors prêt pour commencer la pré-définition du mouvement afin de contourner un obstacle lors de sa détection.

Pré-définition du mouvement

Premier approche

Nous allons premièrement faire tourner le robot lorsqu'il rencontre un obstacle :

Tourner à droite

void loop()
{
  Capteur();
  if (d < 40)
  {
    driveArdumoto(MOTOR_A,CW,100);
    driveArdumoto(MOTOR_B,CW,100);
  }
  else
  {
    driveArdumoto(MOTOR_A,CCW,100);
    driveArdumoto(MOTOR_B,CW,100);
  }
}

Cependant, le résultat obtenu n'est pas celui espéré : lorsque le robot détecte l'obstacle, il tourne à droite, mais une fois qu'il n'y a plus d'obstacle devant lui, il avance.

Lorsqu'on place notre robot dans la pièce utilisée pour NAO, il tourne lorsqu'il s'approche trop près d'un mur, ce programme est alors nommé "ProgRobAspi" pour programme robot aspirateur. Il n'est pas possible de définir un mouvement avec un simple test de distance.

Second approche

Après réflexion, il fallait que le robot puisse déterminer des points particuliers, c'est à dire lorsqu'il ne détectait rien puis qu'un obstacle apparaissait, et lorsqu'il détectait un obstacle puis qu'il ne détectait plus rien : on utilisera alors le principe de l'état présent et passé.

On a alors le programme suivant :

Esquive de l'obstacle

void setup()
{
  d=500000;
  delay(2000);
  setupArdumoto(); // Set all pins as outputs
  setupCapteur();
}

void loop()
{
  Capteur();
  Etat1 = Etat2;

  if (d < 15) Etat2 = 1;
  else Etat2 = 0;

  if (Etat1 == 0 && Etat2 == 0)
  {
    driveArdumoto(MOTOR_A, CCW, 70);
    driveArdumoto(MOTOR_B, CW, 70);
  }
  if (Etat1 == 1 && Etat2 == 1)
  {
    droite();
    courbedroite();
    droite2();
  }
}

void courbedroite()
{
  driveArdumoto(MOTOR_A, CCW, 140);
  driveArdumoto(MOTOR_B, CW, 110);
  delay(1900);

}
void droite2()
{
  driveArdumoto(MOTOR_A, CW, 100); 
  driveArdumoto(MOTOR_B, CW, 100);
  delay(250); //delay différent en raison de la modification de vitesse soudaine
}
void droite()
{
  driveArdumoto(MOTOR_A, CW, 100); // tourner droite 45°
  driveArdumoto(MOTOR_B, CW, 100);
  delay(400);
}

On a alors un robot capable d'éviter un obstacle qu'il rencontre. Cependant, ce programme ne marche pas dans toutes les conditions possibles : il ne marchera que si les obstacles sont alignés et selon les frottements des roues par rapport au sol (le bois marche très bien), il ne détectera pas d'autres obstacles lorsqu'il sera dans sa phase d'esquive et il perdra alors son objectif.

Solutions à apporter

Afin d'obtenir le robot espéré pour la compétition, il faudra qu'il puisse détecter un obstacle pendant son mouvement prédéfini. Pour cela, ajouter un "if" est envisageable, ainsi qu'un autre état (Etat3). De plus, utiliser un seul capteur est suffisant, mais pas assez précis, il est dont envisageable d'ajouter un ou deux capteurs à ultrason supplémentaires pour améliorer cette précision.

Carte d'extension

Pour faciliter la connexion entre les différents capteur et la carte Arduino on a crée une carte d’extension qu'on vas connecter entre la carte Arduino et Ardumoto. Sur cette carte il existe 3 bornier de 4 pour les capteurs ultrasons et un bornier de 3 pour la carte de détection d'arrivée.