Ligne 9 : |
Ligne 9 : |
| 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 // 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); |
| + | } |
| + | } |
| + | |
| + | </syntaxhighlight> |
| {{Pololu-Zumo-Shield-Arduino-TRAILER}} | | {{Pololu-Zumo-Shield-Arduino-TRAILER}} |