Pololu-Zumo-Shield-Arduino-detecter-collision

Cet exemple étend le projet de 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 Section 3.d) pour détecter les collisions. With the 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 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 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.

#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 // write log output to serial port

#define LED 13
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12

// Accelerometer Settings
#define RA_SIZE 3  // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400  // for detection of contact (~16000 = magnitude of acceleration due to gravity)

// Reflectance Sensor Settings
#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];
// this might need to be tuned for different lighting conditions, surfaces, etc.
#define QTR_THRESHOLD  1500 // microseconds
ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN); 

// Motor Settings
ZumoMotors motors;

// these might need to be tuned for different motor types
#define REVERSE_SPEED     200 // 0 is stopped, 400 is full speed
#define TURN_SPEED        200
#define SEARCH_SPEED      200
#define SUSTAINED_SPEED   400 // switches to SUSTAINED_SPEED from FULL_SPEED after 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;  // current forward speed setting
unsigned long full_speed_start_time;
#define FULL_SPEED_DURATION_LIMIT     250  // ms

// Sound Effects
ZumoBuzzer buzzer;
const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody
 // use V0 to suppress sound effect; v15 for max volume
 
 // Timing
unsigned long loop_start_time;
unsigned long last_turn_time;
unsigned long contact_made_time;
#define MIN_DELAY_AFTER_TURN          400  // ms = min delay before detecting contact event
#define MIN_DELAY_BETWEEN_CONTACTS   1000  // ms = min delay between detecting new contact event

// RunningAverage class 
// based on RunningAverage library for 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;
};

// Accelerometer Class -- extends the LSM303 Library to support reading and averaging the x-y acceleration 
//   vectors from the onboard LSM303DLHC accelerometer/magnetometer
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;  // set when accelerometer detects contact with opposing robot

// forward declaration
void setForwardSpeed(ForwardSpeed speed);

void setup()
{  
  // Initiate the Wire library and join the I2C bus as a master
  Wire.begin();
  
  // Initiate LSM303
  lsm303.init();
  lsm303.enable();
  
#ifdef LOG_SERIAL
  Serial.begin(9600);
  lsm303.getLogHeader();
#endif

  randomSeed((unsigned int) millis());
  
  // uncomment if necessary to correct motor directions
  //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);
   
  // play audible countdown
  for (int i = 0; i < 3; i++)
  {
    delay(1000);
    buzzer.playNote(NOTE_G(3), 50, 12);
  }
  delay(1000);
  buzzer.playFromProgramSpace(sound_effect);
  delay(1000);
  
  // reset loop variables
  in_contact = false;  // 1 if contact made; 0 if no contact or contact lost
  contact_made_time = 0;
  last_turn_time = millis();  // prevents false contact detection on initial acceleration
  _forwardSpeed = SearchSpeed;
  full_speed_start_time = 0;
}

void loop()
{
  if (button.isPressed())
  {
    // if button is pressed, stop and wait for another press to go again
    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)
  {
    // if leftmost sensor detects line, reverse and turn to the right
    turn(RIGHT, true);
  }
  else if (sensor_values[5] < QTR_THRESHOLD)
  {
    // if rightmost sensor detects line, reverse and turn to the left
    turn(LEFT, true);
  }
  else  // otherwise, go straight
  {
    if (check_for_contact()) on_contact_made();
    int speed = getForwardSpeed();
    motors.setSpeeds(speed, speed);
  }
}

// execute turn 
// direction:  RIGHT or LEFT
// randomize: to improve searching
void turn(char direction, bool randomize)
{
#ifdef LOG_SERIAL
  Serial.print("turning ...");
  Serial.println();
#endif

  // assume contact lost
  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;
}
  
// check for contact, but ignore readings immediately after turning or losing 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);
}

// sound horn and accelerate on contact -- 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);
}

// reset forward speed
void on_contact_lost()
{
#ifdef LOG_SERIAL
  Serial.print("contact lost");
  Serial.println();
#endif
  in_contact = false;
  setForwardSpeed(SearchSpeed);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
  // Enable Accelerometer
  // 0x27 = 0b00100111
  // Normal power mode, all axes enabled
  writeAccReg(LSM303::CTRL_REG1_A, 0x27);

  if (getDeviceType() == LSM303::device_DLHC)
  writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

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;
}



// RunningAverage class 
// based on RunningAverage library for Arduino
// source:  http://playground.arduino.cc/Main/RunningAverage
// author:  Rob.Tillart@gmail.com
// Released to the 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);
}

// resets all counters
template <typename T>
void RunningAverage<T>::clear() 
{ 
  _cnt = 0;
  _idx = 0;
  _sum = zero;
  for (int i = 0; i< _size; i++) _ar[i] = zero;  // needed to keep addValue simple
}

// adds a new value to the data-set
template <typename T>
void RunningAverage<T>::addValue(T f)
{
  _sum -= _ar[_idx];
  _ar[_idx] = f;
  _sum += _ar[_idx];
  _idx++;
  if (_idx == _size) _idx = 0;  // faster than %
  if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template <typename T>
T RunningAverage<T>::getAverage() const
{
  if (_cnt == 0) return zero; // NaN ?  math.h
  return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template <typename T>
void RunningAverage<T>::fillValue(T value, int number)
{
  clear();
  for (int i = 0; i < number; i++) 
  {
    addValue(value);
  }
}

---

Basé sur "Zumo Shield for Arduino" de Pololu (www.pololu.com/docs/0J57) - Traduit en Français par shop.mchobby.be CC-BY-SA pour la traduction
Toute copie doit contenir ce crédit, lien vers cette page et la section "crédit de traduction". Traduit avec l'autorisation expresse de Pololu (www.pololu.com)

Based on "Zumo Shield for Arduino" from Pololu (www.pololu.com/docs/0J57) - Translated to French by shop.mchobby.be CC-BY-SA for the translation
Copies must includes this credit, link to this page and the section "crédit de traduction" (translation credit). Translated with the Pololu's authorization (www.pololu.com)