Robots suiveurs 3 - Code RobotCamera

#include <webots/Robot.hpp>
#include <webots/Emitter.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/motor.hpp>
#include <webots/Camera.hpp>
#include <iostream>
#include <stdio.h>
#include <string.h>

#define COMMUNICATION_CHANNEL 1
#define MAX_SPEED 6.28

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

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

  // créé 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");
  Motor *left_motor = robot->getMotor("left wheel motor");
  Motor *right_motor = robot->getMotor("right wheel motor");
  Camera *camera = robot->getCamera("camera");
  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);
  }
  
  // active la caméra
  camera->enable(timeStep);

  // initialisation du moteur
  left_motor->setPosition(INFINITY);
  right_motor->setPosition(INFINITY);
  left_motor->setVelocity(0.0);
  right_motor->setVelocity(0.0);
  
  // si mauvais canal
  const int channel = emitter->getChannel();
    if (channel != COMMUNICATION_CHANNEL) 
    {
      emitter->setChannel(COMMUNICATION_CHANNEL);
    }
    
  emitter->setRange(0.4); // définit la plage de l'émetteur
  
  int exPos = 0; // utilisé pour définir dans quel sens tourner quand on voit plus le robot rouge
  
  double vitesse = 0; // utilisé pour faire la réduction de vitesse
  
  // utilisés pour faire l'évitement d'obstacles
  int evitementG = 0;
  int incrementEvitemment = 0;

 // boucle principale
 
 // Le but est de séparer l'image en 3 partie : une partie centrale, gauche et droite
 // et d'analyser le nombre de pixel rouge dans chacune de ces parties
 // Si le nombre de pixel rouge est plus important au centre, on avance, si il l'est plus à 
 // gauche, on tourne à gauche, etc...
 
  while (robot->step(timeStep) != -1) {
  
      // envoie un message terminé par null
      const char *message = "Hello!";
      emitter->send(message, strlen(message) + 1);
  
    // crée des variables pour compter la somme des composantes rouge, verte et bleue des pixels pour chacune des 3 parties de l'image étudiée
    int red = 0;
    int blue = 0;
    int green = 0;
        
    int redg = 0;
    int blueg = 0;
    int greeng = 0;
    
    int redd = 0;
    int blued = 0;
    int greend = 0;
    
    int redPleinCadran = 0;
    int reddPleinCadran = 0;
    int redgPleinCadran = 0;
    
    int bluePleinCadran = 0;
    int bluedPleinCadran = 0;
    int bluegPleinCadran = 0;
    
    int greenPleinCadran = 0;
    int greendPleinCadran = 0;
    int greengPleinCadran = 0;
    
    
    // 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 ;
  
    bool left_obstacle =
      psValues[5] > 100.0 ||
      psValues[6] > 100.0 ;
   
    bool front_obstacle3 = right_obstacle || left_obstacle;
    
    double valMax = std::max(psValues[7], psValues[0]);
    
    // Booléen servant à éviter une collision avec un objet en face: Seuil 1
    bool front_obstacle1 = psValues[7] > 200 || psValues[0] > 200;
    // Booléen servant à éviter une collision avec un objet en face : Seuil 2
    bool front_obstacle2 = psValues[7] > 100 || psValues[0] > 100;  
    
    // Vitesse roue gauche et droite
    double leftSpeed = 0.5*MAX_SPEED;
    double rightSpeed = 0.5*MAX_SPEED;
    
    int width = camera -> getWidth();
    int height = camera -> getHeight();
    // Récupération de l'image de la caméra
    const unsigned char *image = camera -> getImage();
    
    // Première boucle servant à diviser l'image en 3 parties (partie centrale)
    for (int i = 2* width / 5; i < 3 * width / 5; i++) {
        for (int j = 2*height / 5; j < 3* height / 5; j++) {
          red += camera -> imageGetRed(image, width, i, j);
          blue += camera -> imageGetBlue(image, width, i, j);
          green += camera -> imageGetGreen(image, width, i, j);
        }
        for (int j = 0; j <  height; j++) {
          redPleinCadran += camera -> imageGetRed(image, width, i, j);
          greenPleinCadran += camera -> imageGetGreen(image, width, i, j);
           bluePleinCadran += camera -> imageGetBlue(image, width, i, j);
        }
    }
    // Deuxième boucle (partie droite)
    for (int i = 3* width / 5; i < width; i++) {
        for (int j = 2*height / 5; j <  3*height/5 ; j++) {
          redd += camera -> imageGetRed(image, width, i, j);
          blued += camera -> imageGetBlue(image, width, i, j);
          greend += camera -> imageGetGreen(image, width, i, j);
        }
        for (int j = 0; j <  height; j++) {
          reddPleinCadran += camera -> imageGetRed(image, width, i, j);
          greendPleinCadran += camera -> imageGetGreen(image, width, i, j);
          bluedPleinCadran += camera -> imageGetBlue(image, width, i, j);
        }
    }
    // Troisième boucle (partie gauche)
    for (int i = 0 ; i < 2*width/5; i++) {
        for (int j = 0; j <  2*height/5 ; j++) {
          redg += camera -> imageGetRed(image, width, i, j);
          blueg += camera -> imageGetBlue(image, width, i, j);
          greeng += camera -> imageGetGreen(image, width, i, j);
        }
        for (int j = 0; j <  height; j++) {
          redgPleinCadran += camera -> imageGetRed(image, width, i, j);
          greengPleinCadran += camera -> imageGetGreen(image, width, i, j);
          bluegPleinCadran += camera -> imageGetBlue(image, width, i, j);
        }
    }

    if (evitementG == 0){
    
    // Si on a plus de pixel rouges au milieu, alors...
    if ((red > 1.5*green) && (red > 1.5*blue) ){
      // Si on a un obstacle devant alors on se stoppe
      if (front_obstacle3){
        if (front_obstacle1){
           if (right_obstacle && reddPleinCadran> 2*bluedPleinCadran && reddPleinCadran> 2*greendPleinCadran){
                left_motor->setVelocity(0.0);
                right_motor->setVelocity(0.0);       
           }
           else if (left_obstacle && redgPleinCadran> 2*bluegPleinCadran && redgPleinCadran> 2*greengPleinCadran){
                  left_motor->setVelocity(0.0);
                  right_motor->setVelocity(0.0);
           }
           else {
                left_motor->setVelocity(0.0);
                right_motor->setVelocity(0.0);
                evitementG=1;
           }
         }
        // Si on se rapproche d'un obstacle, on ralentit
        else if (front_obstacle2){
          double produitEnCroix = ((200-valMax)/(double)100);
          vitesse = produitEnCroix * 0.5 * MAX_SPEED;
          left_motor->setVelocity(vitesse);
          right_motor->setVelocity(vitesse);
          }
         }
      // Sinon on avance
      else{
        left_motor->setVelocity(leftSpeed);
        right_motor->setVelocity(rightSpeed);
      }
   
    }
    // Si il n'y a rien au milieu
    else{
    
      // Si on a plus de pixel rouge à droite alors on tourne à droite
      if((redd > 1.5*greend) && (redd > 1.5*blued)){
       
        left_motor->setVelocity(leftSpeed);
        right_motor->setVelocity(-rightSpeed);
        exPos = 0;
        
      }
      // Sinon si on a plus de pixel rouge à gauche alors on tourne à gauche
      else{
        if((redg > 1.5*greeng) && (redg > 1.5*blueg)){
          left_motor->setVelocity(-leftSpeed);
          right_motor->setVelocity(rightSpeed);
          exPos = 1;
          
        }
        // Sinon on tourne sur nous même pour essayer de trouver un objet rouge
        else{
          if (exPos==1) {
            left_motor->setVelocity(-leftSpeed);
            right_motor->setVelocity(rightSpeed);
            
          }
          else {
            left_motor->setVelocity(leftSpeed);
            right_motor->setVelocity(-rightSpeed);
            
          }
        }
      }
    }
    }
    else{
    
    //évitement
      if (left_obstacle) {
        // tourner à gauche si obstacle à droite
        left_motor -> setVelocity(leftSpeed);
        right_motor -> setVelocity(-leftSpeed);
        exPos = 1;
        }    
       else if (right_obstacle) {
        // tourner à droite si obstacle à gauche 
        left_motor -> setVelocity(-rightSpeed);
        right_motor -> setVelocity(rightSpeed);
        exPos = 0;
      } 
      // Lorsque l'obstacle n'entrave plus la progression, on avance
      else if (incrementEvitemment <30){
          left_motor->setVelocity(0.5 * MAX_SPEED);
          right_motor->setVelocity(0.5 * MAX_SPEED);
          incrementEvitemment++;
          } 
      else{
      //Réinitialisation des flags et incréments
      incrementEvitemment=0;
      evitementG=0;
      }
        
    
    }
    

   }; 


  // Destruction des objets et fin de programme.
  delete robot;
  return 0;
}