Modifications

Sauter à la navigation Sauter à la recherche

Pololu-Zumo-Shield-Arduino-detecter-collision (voir la source)

Version du 17 avril 2017 à 21:57

15 480 octets ajoutés ,  17 avril 2017 à 21:57
→‎Code
Ligne 8 : Ligne 8 :     
Pour en apprendre plus sur le fonctionnement du programme vous pouvez lire les commentaire du croquis {{fname|SumoCollisionDetect.ino}}.
 
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}}
 
{{Pololu-Zumo-Shield-Arduino-TRAILER}}
Admin
Bureaucrates, Administrateurs
29 917

modifications

Récupérée de « https://wiki.mchobby.be/index.php?title=Spécial:MobileDiff/22571...22612 »

Menu de navigation