Robots suiveurs 3 - Code RobotReceiver

#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 2
#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
  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"
  };
  
  // active les capteurs de distance
  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
  const int channel = receiver->getChannel();
    if (channel != COMMUNICATION_CHANNEL) 
    {
      receiver->setChannel(COMMUNICATION_CHANNEL);
    }
  
  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;
  
    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 2 communcating with follower 1\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 2 NOT communcating with follower 1\n");
          message_printed = 2;
        }
     }
  };

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