Robots suiveurs 3 - Code Communication Intermédiaire (esclave)

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

#define COMMUNICATION_CHANNEL 1
#define MAX_SPEED 6.28

// All the webots classes are defined in the "webots" namespace
using namespace webots;

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

  // create the Robot instance.
  Robot *robot = new Robot();

  // get a handler to the peripherials
  Receiver *receiver = robot->getReceiver("receiver");
  Motor *left_motor = robot->getMotor("left wheel motor");
  Motor *right_motor = robot->getMotor("right wheel motor");
  
  int message_printed = 0; // used to avoid printing continuously the communication state

  //Motor initialisation
  left_motor->setVelocity(0);
  right_motor->setVelocity(0);
  left_motor->setPosition(INFINITY);
  right_motor->setPosition(INFINITY);
  
  // get the time step of the current world.
  int timeStep = (int)robot->getBasicTimeStep();
  
  //if wrong channel
  const int channel = receiver->getChannel();
    if (channel != COMMUNICATION_CHANNEL) 
    {
      receiver->setChannel(COMMUNICATION_CHANNEL);
    }
  
  receiver->enable(timeStep); //enables the receiver

  // Main loop
  while (robot->step(timeStep) != -1) 
  {
    if (receiver->getQueueLength() > 0) // is there at least one packet in the receiver's queue ?
    {
        // read receivers data 
      const double *position=receiver->getEmitterDirection();
      double signal=receiver->getSignalStrength();
     
      if (message_printed != 1) 
      {
        printf("Communicating\n");
        message_printed = 1;
      }
          
      if(signal>150) //Stop the robot if it's too close to the master
      {
        left_motor->setVelocity(0);
        right_motor->setVelocity(0);
      }
      else{
        //Following behavior
        if (position[0]<-0.1)
        {
          left_motor->setVelocity(0);
          right_motor->setVelocity(MAX_SPEED);
        }
        else if(position[0]>0.1)
        {
          left_motor->setVelocity(MAX_SPEED);
          right_motor->setVelocity(0);
        }
        else 
        {
          left_motor->setVelocity(MAX_SPEED);
          right_motor->setVelocity(MAX_SPEED);
        }
      }  
           
      // fetch next packet 
      receiver->nextPacket();
      
     } 
    
     else 
     {
        if (message_printed != 2) 
        {
          printf("Not communicating\n");
          message_printed = 2;
        }
     }
  };

  // Exit cleanup code.
  delete robot;
  return 0;
}