Robots suiveurs 3 - Code emitter-receiver

#include <webots/Robot.hpp>
#include <webots/Emitter.hpp>
#include <webots/Receiver.hpp>
#include <webots/Motor.hpp>
#include <stdio.h>
#include <string.h>
#include <webots/DistanceSensor.hpp>

#define COMMUNICATION_CHANNEL_EMITTER 2
#define COMMUNICATION_CHANNEL_RECEIVER 1
#define MAX_SPEED 6.28

// toutes les classes webots sont définies dans le namespace "webots"
using namespace webots;

int main(int argc, char **argv) {

  // crée l'instance Robot
  Robot *robot = new Robot();
  
  // récupère le pas de temps du monde actuel
  int timeStep = (int)robot->getBasicTimeStep();

  // crée les instances des périphériques et les associe
  Emitter *emitter = robot->getEmitter("emitter");
  Receiver *receiver = robot->getReceiver("receiver");
  Motor *left_motor = robot->getMotor("left wheel motor");
  Motor *right_motor = robot->getMotor("right wheel motor");
  DistanceSensor *ps[8];
  char psNames[8][4] = {
    "ps0", "ps1", "ps2", "ps3",
    "ps4", "ps5", "ps6", "ps7"
  };
  for (int i = 0; i < 8; i++) {
    ps[i] = robot->getDistanceSensor(psNames[i]);
    ps[i]->enable(timeStep);
  }
  
  int message_printed = 0; // utilisé pour éviter d'imprimer en continu l'état de communication

  // initialisation du moteur
  left_motor->setVelocity(0);
  right_motor->setVelocity(0);
  left_motor->setPosition(INFINITY);
  right_motor->setPosition(INFINITY);
  
  
  // si mauvais canal de communication
  const int channel_receiver = receiver->getChannel();
    if (channel_receiver != COMMUNICATION_CHANNEL_RECEIVER) 
    {
      receiver->setChannel(COMMUNICATION_CHANNEL_RECEIVER);
    }
   
  const int channel_emitter = emitter->getChannel();
    if (channel_emitter != COMMUNICATION_CHANNEL_EMITTER) 
    {
      emitter->setChannel(COMMUNICATION_CHANNEL_EMITTER);
    }
    
   emitter->setRange(0.4); // définit la plage de l'émetteur
   
   receiver->enable(timeStep); // active le récepteur
 
  // boucle principale
  while (robot->step(timeStep) != -1) 
  {
  
    // lit les sorties des capteurs de distance
    double psValues[8];
    for (int i = 0; i < 8 ; i++)
      psValues[i] = ps[i]->getValue();

    // détecte les obstacles
    bool right_obstacle =
      psValues[0] > 100.0 ||
      psValues[1] > 100.0 ||
      psValues[2] > 100.0;
    bool left_obstacle =
      psValues[5] > 100.0 ||
      psValues[6] > 100.0 ||
      psValues[7] > 100.0;
  
  
   // envoie un message terminé par null
   const char *message = "Hello!";
   emitter->send(message, strlen(message) + 1);
  
  
    if (receiver->getQueueLength() > 0) // y a-t-il au moins un paquet dans la file d'attente du récepteur ?
    {
      // lit les données des récepteurs
      const double *position=receiver->getEmitterDirection();
      double signal=receiver->getSignalStrength();
     
      if (message_printed != 1) 
      {
        printf("Follower 1 communcating with master\n");
        message_printed = 1;
      }
          
      if(signal>80) // arrete le robot s'il est trop proche du maître
      {
        left_motor->setVelocity(0);
        right_motor->setVelocity(0);
      }
      else if (signal>50){ // réduit la vitesse du robot progressivement lorsqu'il s'approche du maître
          double produitEnCroix = ((80-signal)/(double)30);
          double vitesse = produitEnCroix * 0.5 * MAX_SPEED;
          left_motor->setVelocity(vitesse);
          right_motor->setVelocity(vitesse);
      }
      
      else{
      
      if (left_obstacle) {
        // tourne à droite
        left_motor->setVelocity(0.5 * MAX_SPEED);
        right_motor->setVelocity(0);
      }
      else if (right_obstacle) {
        // tourne à gauche
        left_motor->setVelocity(0);
        right_motor->setVelocity(0.5 * MAX_SPEED);
       }

        // comportement suiveur
        else if (position[0]<-0.1)
        {
          left_motor->setVelocity(0);
          right_motor->setVelocity(0.5 * MAX_SPEED);
        }
        else if(position[0]>0.1)
        {
          left_motor->setVelocity(0.5 * MAX_SPEED);
          right_motor->setVelocity(0);
        }
        else 
        {
          left_motor->setVelocity(0.5 * MAX_SPEED);
          right_motor->setVelocity(0.5 * MAX_SPEED);        
        }
      }  
           
      // récupère le prochain paquet
      receiver->nextPacket();
      
     } 
    
     else 
     {
        if (message_printed != 2) 
        {
          printf("Follower 1 NOT communcating with master\n");
          message_printed = 2;
        }
     }
  };

  // code de nettoyage
  delete robot;
  return 0;
}