Robots suiveurs 3 - Code de robot suiveur via l'utilisation de capteurs

#include <webots/Robot.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>

// time in [ms] of a simulation step
#define TIME_STEP 64

#define MAX_SPEED 6.28

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

// entry point of the controller
int main(int argc, char **argv) {
  // create the Robot instance.
  Robot *robot = new Robot();

  // initialize devices
  DistanceSensor *ps[2];
  char psNames[2][4] = {
    "ps0", "ps7"
  };

  for (int i = 0; i < 2; i++) {
    ps[i] = robot->getDistanceSensor(psNames[i]);
    ps[i]->enable(TIME_STEP);
  }

  Motor *leftMotor = robot->getMotor("left wheel motor");
  Motor *rightMotor = robot->getMotor("right wheel motor");
  leftMotor->setPosition(INFINITY);
  rightMotor->setPosition(INFINITY);
  leftMotor->setVelocity(0.0);
  rightMotor->setVelocity(0.0);

  // feedback loop: step simulation until an exit event is received
  while (robot->step(TIME_STEP) != -1) {
    // read sensors outputs
    double psValues[2];
    for (int i = 0; i < 2 ; i++)
      psValues[i] = ps[i]->getValue();

    // detect obstacles
    bool robot_right =
      psValues[0] > 80.0;
    bool robot_left =
      psValues[1] > 80.0;

    // initialize motor speeds at 50% of MAX_SPEED.
    double leftSpeed  = 0.5 * MAX_SPEED;
    double rightSpeed = 0.5 * MAX_SPEED;
    // modify speeds according to obstacles
    if (robot_left && robot_right){
      leftSpeed  = 0;
      rightSpeed = 0;
    }
    else if (robot_left) {
      // turn right
      leftSpeed  -= 0.5 * MAX_SPEED;
      rightSpeed += 0.5 * MAX_SPEED;
    }
    else if (robot_right) {
      // turn left
      leftSpeed  += 0.5 * MAX_SPEED;
      rightSpeed -= 0.5 * MAX_SPEED;
    }
    // write actuators inputs
    leftMotor->setVelocity(leftSpeed);
    rightMotor->setVelocity(rightSpeed);
  }

  delete robot;
  return 0; //EXIT_SUCCESS
}