Voir la source de Pololu-Zumo-Shield-Arduino-detecter-collision

{{Pololu-Zumo-Shield-Arduino-NAV}}

Cet exemple étend le projet de [[Pololu-Zumo-Shield-Arduino-detecter-bord|détection simplifiée des bordures]] du robot sumo tel que décrit dans la section précédente. Cet exemple utilise l'accéléromètre du shield Zumo (le LSM303, boussole 3 axes, décrit dans la [[Pololu-Zumo-Shield-Arduino-seneur-inertiel|Section 3.d]]) pour détecter les collisions. With the [[Pololu-Zumo-Shield-Arduino-bibliotheque-Zumo|Zumo Shield Arduino Libraries]] installed, the sketch file can be opened in Arduino by selecting '''Fichier > Exemples > ZumoExamples > SumoCollisionDetect'''. 

Cet exemple requière l'installation de la [https://github.com/pololu/lsm303-arduino|bibliothèque LSM303] .

Ce croquis/exemple utilise les composantes X et Y du vecteur d'accélération mesuré à l'aide du LSM303 afin détecter une collision avec un robot adversaire. Lorsqu'il détecte un contacte, le Zumo accélère, ce qui lui permet d'éjecter l'opposant hors du ring avec plus d'efficacité. Accélérer permet également d'échapper à l'opposant si c'est ce dernier vient percuter le Zumo sous un angle indésirable (ex: par l'arrière). 

Pour en apprendre plus sur le fonctionnement du programme vous pouvez lire les commentaire du croquis {{fname|SumoCollisionDetect.ino}}.

== Code ==
Voici une copie de l'exemple avec traduction des commentaires pour vous aider à mieux comprendre le fonctionnement du croquis/sketch.

Nous recommandons de toujours charger l'exemple depuis les codes d'exemples de la bibliothèque Zumo.

<syntaxhighlight lang="c">
#include <ZumoBuzzer.h>
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <avr/pgmspace.h>
#include <Wire.h>
#include <LSM303.h>

/* Cet exemple utilise l'accéléromètre du shield Zumo (LSM303DLHC) avec la bibliothèque pour 
 * détecter les contact avec un robot adversaire sur un ring sumo. La bibliothèque LSM303 
 * n'est pas incluse dans dans les bibliothèques du shield Zumo; 
 * Elle peut être téléchargée séparément sur GitHub: 
 *
 *    https://github.com/pololu/LSM303 
 *
 * Cet exemple étend le code de BorderDetect, qui utilise le réseau de senseur du Zumo
 * et bibliothèque pour détecter la bordure du ring sumo ring. Il illustre également l'utilisation  
 * des bibliothèques ZumoMotors, PushButton et ZumoBuzzer.
 *
 * Dans la loop(), le programme lit les composantes x et y de l'accélération (ignorant la composante z),
 * et détecte un contact quand la moyenne, sur 3 périodes, de la magnitude du vecteur x-y 
 * excède la valeur du seuil XY_ACCELERATION_THRESHOLD (déterminé de façon empirique).
 * Lors de la détection d'une collision, la vitesse en marche avant est augmenté de la  
 * valeur par défaut SEARCH_SPEED (vitesse de recherche) à à  FULL_SPEED (pleine vitesse).
 * Cela permet de simuler une réponse de type "fight or flight" (frapper ou s'échapper).
 *
 * Le programme essayer de détecter le contact uniquement lorsque le Zumo avance en ligne droite.
 * Lorsque le Zumo tourne après une détection de la bordure du ring, la fait de tourner génère une 
 * accélération sur le plan x-y, ce qui rend l'interprétation des lectures difficiles (entre
 * rotation volontaire et détection de collision).
 * Etant donné que le Zumo accélère également lorsqu'il redémarre en ligne droit, les lectures
 * d'accélération sont également ignorés durant la période de MIN_DELAY_AFTER_TURN millisecondes
 * après avoir terminé sa rotation. (MIN_DELAY_AFTER_TURN : delais minimum après rotation)
 *
 * Pour éviter d'autres faux positifs, une valeur MIN_DELAY_BETWEEN_CONTACTS est également 
 * spécifiée. (MIN_DELAY_BETWEEN_CONTACTS : délais minimum entre contacts)
 *
 * Cet exemple contient également les améliorations suivantes:
 * 
 *  - Utilise la bibliothèque Zumo Buzzer pour jouer un effet sonore (Mélodie de "charge") au
 *    début de la compétition et lorsqu'un contact est fait l'adversaire.
 *
 *  - L'angle de rotation (lors de la détection des boards) est choisit au hasard. Ce qui permet
 *    au Zumo d'avoir un schéma de recherche plus efficace.
 *
 *  - Support de FULL_SPEED_DURATION_LIMIT (temps max à pleine vitesse), permettant au robot de 
 *    passer à la vitesse SUSTAINED_SPEED (vitesse soutenue) après une courte période de 
 *    mouvement à la vitesse FULL_SPEED (pleine vitesse). Dans cet exemple, les deux vitesses sont
 *    fixées au Maximum de 400, mais cette fonctionnalité peut être utile pour éviter de déraper 
 *    au moment de tourner (sur le bord du ring) si la surface du ring est anormalement lisse.
 *
 *  - Log des données de l'accéléromètre vers le moniteur sérue lorsque LOG_SERIAL est #defined.
 *
 *  Cette exemple utilise également la bibliothèque RunningAverage (domaine publique) du site Arduino; 
 *  Le code utile a été copié dans ce fichier .ino. Vous n'avez donc pas besoin de le 
 *  télécharger séparément .
 */

// #define LOG_SERIAL // écrire la sortie du log vers le port série

#define LED 13
Pushbutton button(ZUMO_BUTTON); // bouton poussoir sur la broche 12

// Paramètre de l'accéléromètre
#define RA_SIZE 3  // Nombre de lectures à inclure dans la moyenne de accélération durant la phase de déplacement (running average of accelerometer readings)
#define XY_ACCELERATION_THRESHOLD 2400  // pour la détection de contact (~16000 = mangitude de l'accélération du à la gravité)

// Configuration du réseau senseur infrarouge (Reflectance Sensor)
#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];
// Ce paramètre pourrait avoir besoin d'être mis au point en fonction des conditions de luminosité, surface, etc.
#define QTR_THRESHOLD  1500 // microseconds
ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN); 

// Configuration moteur
ZumoMotors motors;

// Ces valeurs pourrait avoir besoin d'être mis-au-pnt en fonction des différents types de moteur
#define REVERSE_SPEED     200 // 0 = à l'arrêt, 400  = pleine vitesse
#define TURN_SPEED        200
#define SEARCH_SPEED      200
#define SUSTAINED_SPEED   400 // Vitesse soutenue SUSTAINED_SPEED après la vitesse FULL_SPEED appliquée durant FULL_SPEED_DURATION_LIMIT ms
#define FULL_SPEED        400
#define STOP_DURATION     100 // ms
#define REVERSE_DURATION  200 // ms
#define TURN_DURATION     300 // ms

#define RIGHT 1
#define LEFT -1

enum ForwardSpeed { SearchSpeed, SustainedSpeed, FullSpeed };
ForwardSpeed _forwardSpeed;  // Configuration de la vitesse actuelle
unsigned long full_speed_start_time;
#define FULL_SPEED_DURATION_LIMIT     250  // ms

// Effet sonore
ZumoBuzzer buzzer;
const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // Mélodie de "charge"
 // Utiliser V0 pour supprimer le son; v15 pour volume max
 
 // Minutage
unsigned long loop_start_time;
unsigned long last_turn_time;
unsigned long contact_made_time;
#define MIN_DELAY_AFTER_TURN          400  // ms = délais minimum avant la détection d'un événement collision
#define MIN_DELAY_BETWEEN_CONTACTS   1000  // ms = délais minimum entre avant la détection d'un nouvel événement collision

// classe RunningAverage  
// Basé sur la bibliothèque RunningAverage pour Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
template <typename T> 
class RunningAverage
{
  public:
    RunningAverage(void);
    RunningAverage(int);
    ~RunningAverage();
    void clear();
    void addValue(T);
    T getAverage() const;
    void fillValue(T, int);
  protected:
    int _size;
    int _cnt;
    int _idx;
    T _sum;
    T * _ar;
    static T zero;
};

// Classe Accelerometer -- étend la bibliothèque LSM303 pour supporter la lecture et moyenne des vecteurs
// d'accélérations x-y provenant de l'accéléromètre/magnétomètre LSM303DLHC.
class Accelerometer : public LSM303
{
  typedef struct acc_data_xy
  {
    unsigned long timestamp;
    int x;
    int y;
    float dir;
  } acc_data_xy;
  
  public: 
    Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
    ~Accelerometer() {};
    void enable(void);
    void getLogHeader(void);
    void readAcceleration(unsigned long timestamp);
    float len_xy() const;
    float dir_xy() const;
    int x_avg(void) const;
    int y_avg(void) const;
    long ss_xy_avg(void) const;
    float dir_xy_avg(void) const;
  private:
    acc_data_xy last;
    RunningAverage<int> ra_x;
    RunningAverage<int> ra_y;   
};

Accelerometer lsm303;
boolean in_contact;  // activé lorsque l'accéléromètre détecte un contact avec le robot opposé

// pré-déclaration de setForwardSpeed - vitesse en marche avant
void setForwardSpeed(ForwardSpeed speed);

void setup()
{  
  // Initialise la bibliothèque Wire et joindre le bus I2C comme maître
  Wire.begin();
  
  // Initialiser le LSM303
  lsm303.init();
  lsm303.enable();
  
#ifdef LOG_SERIAL
  Serial.begin(9600);
  lsm303.getLogHeader();
#endif

  randomSeed((unsigned int) millis());
  
  // Dé-commentez les ligne pour corriger le sens de rotation des moteurs (si nécessaire)
  //motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);

  pinMode(LED, HIGH);
  buzzer.playMode(PLAY_AUTOMATIC);
  waitForButtonAndCountDown(false);
}

void waitForButtonAndCountDown(bool restarting)
{ 
#ifdef LOG_SERIAL
  Serial.print(restarting ? "Restarting Countdown" : "Starting Countdown");
  Serial.println();
#endif
  
  digitalWrite(LED, HIGH);
  button.waitForButton();
  digitalWrite(LED, LOW);
   
  // jouer le décompte audio
  for (int i = 0; i < 3; i++)
  {
    delay(1000);
    buzzer.playNote(NOTE_G(3), 50, 12);
  }
  delay(1000);
  buzzer.playFromProgramSpace(sound_effect);
  delay(1000);
  
  // Réinitialiser les variables de loop()
  in_contact = false;  // 1 si collision; 0 si pas de collision (ou perte de contact)
  contact_made_time = 0;
  last_turn_time = millis();  // Evite les fausses détections de collision durant l'accélération initiale
  _forwardSpeed = SearchSpeed;
  full_speed_start_time = 0;
}

void loop()
{
  if (button.isPressed())
  {
    // Si le bouton est pressé, arrêter et attente une autre pression sur le bouton
    motors.setSpeeds(0, 0);
    button.waitForRelease();
    waitForButtonAndCountDown(true);
  }
  
  loop_start_time = millis();
  lsm303.readAcceleration(loop_start_time); 
  sensors.read(sensor_values);
  
  if ((_forwardSpeed == FullSpeed) && (loop_start_time - full_speed_start_time > FULL_SPEED_DURATION_LIMIT))
  { 
    setForwardSpeed(SustainedSpeed);
  }
  
  if (sensor_values[0] < QTR_THRESHOLD)
  {
    // Si le senseur le plus à gauche détecte une ligne, marche arrière et tourner à droite
    turn(RIGHT, true);
  }
  else if (sensor_values[5] < QTR_THRESHOLD)
  {
    // Si le senseur le plus a droite détecte une ligne, marche arrière et tourner à gauche
    turn(LEFT, true);
  }
  else  // Sinon, aller tout droit
  {
    // vérifier si contact; signaler contact
    if (check_for_contact()) on_contact_made();
    int speed = getForwardSpeed();
    motors.setSpeeds(speed, speed);
  }
}

// Exécuter une rotation 
// direction:  RIGHT (droite) ou LEFT (gauche)
// Angle aléatoire pour améliorer la recherche
void turn(char direction, bool randomize)
{
#ifdef LOG_SERIAL
  Serial.print("turning ...");
  Serial.println();
#endif

  // assume qu'il n'y a pas de contact (pas de collision).
  on_contact_lost();
  
  static unsigned int duration_increment = TURN_DURATION / 4;
  
  // motors.setSpeeds(0,0);
  // delay(STOP_DURATION);
  motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
  delay(REVERSE_DURATION);
  motors.setSpeeds(TURN_SPEED * direction, -TURN_SPEED * direction);
  delay(randomize ? TURN_DURATION + (random(8) - 2) * duration_increment : TURN_DURATION);
  int speed = getForwardSpeed();
  motors.setSpeeds(speed, speed);
  last_turn_time = millis();
}

void setForwardSpeed(ForwardSpeed speed)
{
  _forwardSpeed = speed;
  if (speed == FullSpeed) full_speed_start_time = loop_start_time;
}

int getForwardSpeed()
{
  int speed;
  switch (_forwardSpeed)
  {
    case FullSpeed:
      speed = FULL_SPEED;
      break;
    case SustainedSpeed:
      speed = SUSTAINED_SPEED;
      break;
    default:
      speed = SEARCH_SPEED;
      break;
  }
  return speed;
}
  
// Vérifie s'il y a une collision/contact, mais ignore les lectures immédiatement après une rotation ou une perte de contact
bool check_for_contact()
{
  static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
  return (lsm303.ss_xy_avg() >  threshold_squared) && \
    (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN) && \
    (loop_start_time - contact_made_time > MIN_DELAY_BETWEEN_CONTACTS);
}

// Klaxon et accélération au contact/collision -- frapper ou s'échapper (fight or flight)
void on_contact_made()
{
#ifdef LOG_SERIAL
  Serial.print("contact made");
  Serial.println();
#endif
  in_contact = true;
  contact_made_time = loop_start_time;
  setForwardSpeed(FullSpeed);
  buzzer.playFromProgramSpace(sound_effect);
}

// Réinitialiser la vitesse (de la marche avant)
void on_contact_lost()
{
#ifdef LOG_SERIAL
  Serial.print("contact lost");
  Serial.println();
#endif
  in_contact = false;
  setForwardSpeed(SearchSpeed);
}

// classe Accelerometer -- définition des fonctions membres

// Activer uniquement l'accéléromètre 
// Appeler enableDefault() pour activer l'accéléromètre et le magnétomètre.
void Accelerometer::enable(void)
{
  // Activer l'accéléromètre
  // 0x27 = 0b00100111
  // Mode d'alimentation normal, activer tous les axes
  writeAccReg(LSM303::CTRL_REG1_A, 0x27);

  if (getDeviceType() == LSM303::device_DLHC)
  writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: activer le mode haute résolution
}

void Accelerometer::getLogHeader(void)
{
  Serial.print("millis    x      y     len     dir  | len_avg  dir_avg  |  avg_len");
  Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
  readAcc();
  if (a.x == last.x && a.y == last.y) return;
  
  last.timestamp = timestamp;
  last.x = a.x;
  last.y = a.y;
  
  ra_x.addValue(last.x);
  ra_y.addValue(last.y);
 
#ifdef LOG_SERIAL
 Serial.print(last.timestamp);
 Serial.print("  ");
 Serial.print(last.x);
 Serial.print("  ");
 Serial.print(last.y);
 Serial.print("  ");
 Serial.print(len_xy());
 Serial.print("  ");
 Serial.print(dir_xy());
 Serial.print("  |  ");
 Serial.print(sqrt(static_cast<float>(ss_xy_avg())));
 Serial.print("  ");
 Serial.print(dir_xy_avg());
 Serial.println();
#endif
}

float Accelerometer::len_xy() const
{
  return sqrt(last.x*a.x + last.y*a.y);
}

float Accelerometer::dir_xy() const
{
  return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
  return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
  return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
  long x_avg_long = static_cast<long>(x_avg());
  long y_avg_long = static_cast<long>(y_avg()); 
  return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
  return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
}



// Classe RunningAverage  
// basé sur la bibliothèque RunningAverage d'Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
// autheur: Rob.Tillart@gmail.com
// public domain

template <typename T>
T RunningAverage<T>::zero = static_cast<T>(0);

template <typename T>
RunningAverage<T>::RunningAverage(int n)
{
  _size = n;
  _ar = (T*) malloc(_size * sizeof(T));
  clear();
}

template <typename T>
RunningAverage<T>::~RunningAverage()
{
  free(_ar);
}

// Réinitialiser tous les compteurs
template <typename T>
void RunningAverage<T>::clear() 
{ 
  _cnt = 0;
  _idx = 0;
  _sum = zero;
  for (int i = 0; i< _size; i++) _ar[i] = zero;  // Nécessaire pour maintenir addValue simple
}

// Ajouter une nouvelle valeur au data-set (ensemble des données)
template <typename T>
void RunningAverage<T>::addValue(T f)
{
  _sum -= _ar[_idx];
  _ar[_idx] = f;
  _sum += _ar[_idx];
  _idx++;
  if (_idx == _size) _idx = 0;  // plus rapide que %
  if (_cnt < _size) _cnt++;
}

// Retourne la moyenne des valeurs du data-set
template <typename T>
T RunningAverage<T>::getAverage() const
{
  if (_cnt == 0) return zero; // NaN ?  math.h
  return _sum / _cnt;
}

// Rempli la moyenne avec une valeur
// Le paramètre number détermine ne nombre de fois que la valeur est ajouté (poids)
// number devrait être entre 1 et size (la taille)
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
  clear();
  for (int i = 0; i < number; i++) 
  {
    addValue(value);
  }
}
</syntaxhighlight>

{{Pololu-Zumo-Shield-Arduino-TRAILER}}