Robots suiveurs 3 - Code suivi d'un objet (robot) rouge

Code pour le suivi du robot rouge.

Le principe est le même pour suivre les autres robots de couleur ; il suffit de changer les variables dans les conditions.

// File:          EPuckFollowRed.cpp
// Date:          17/04/2020
// Description:   Le robot suit le cube rouge
// Author:        Mickaël FAURE et Léane GEOFFROY

#include <webots/Robot.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/motor.hpp>
#include <webots/Camera.hpp>
#include <iostream>

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

#define TIME_STEP 64
#define MAX_SPEED 6.28

int main(int argc, char **argv) {
    // On crée une instance Robot :
    Robot *robot = new Robot();
    
    // get the time step of the current world.
    int timeStep = (int)robot->getBasicTimeStep();
    
    // On crée des instances DistanceSensor
    DistanceSensor *ps[8];
    char psNames[8][4] = {
        "ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"
    };
    
    // Association des instances ci-dessus avec les capteurs du robot
    for (int i = 0; i<8; i++){
        ps[i] = robot->getDistanceSensor(psNames[i]);
        ps[i]->enable(TIME_STEP);
    }
    
    // Création d'une instance Camera et association avec la caméra du robot
    Camera *camera = robot->getCamera("camera");
    camera->enable(TIME_STEP);
    
    //Création et association des moteurs
    Motor *leftMotor = robot->getMotor("left wheel motor");
    Motor *rightMotor = robot->getMotor("right wheel motor");
    // Pour régler la vitesse, on fixe la position des roues à l'infini avec
    // une vitesse de rotation de 0
    leftMotor->setPosition(INFINITY);
    rightMotor->setPosition(INFINITY);
    leftMotor->setVelocity(0.0);
    rightMotor->setVelocity(0.0);
    
    int exPos = 0; // 0 droite, 1 gauche
    
    // 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) {
        // On crée des variales pour compter le nombre de pixel rouge, vert et bleu pour chacune
        // des 3 parties
        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;
        
        // Récupération des valeurs des capteurs de position
        double psValues[8];
        for (int i =0;i<8;i++){
            psValues[i] = ps[i]->getValue();
        }
        
        // Booléen servant à éviter une collision avec un objet en face
        bool front_obstacle = psValues[7] > 150 || psValues[0] > 150;
        
        // 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 a diviser l'image en 3 partie (ici 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);
            }
        }
        // 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);
            }
        }
        // Troisième boucle (partie gauche)
        for (int i = 0 ; i < 2*width/5; i++) {
            for (int j = 2*height / 5; j <  3*height/5 ; j++) {
                redg += camera -> imageGetRed(image, width, i, j);
                blueg += camera -> imageGetBlue(image, width, i, j);
                greeng += camera -> imageGetGreen(image, width, i, j);
            }
        }
        
        
        // Si on a plus de pixels rouges au milieu...
        if ((red > 1.5*green) && (red > 1.5*blue) ){
            // Si on a un obstacle devant alors on stop
            if (front_obstacle){
                leftMotor->setVelocity(0.0);
                rightMotor->setVelocity(0.0);
            }
            // Sinon on avance
            else{
                leftMotor->setVelocity(leftSpeed);
                rightMotor->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)){
                leftMotor->setVelocity(leftSpeed);
                rightMotor->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)){
                    leftMotor->setVelocity(-leftSpeed);
                    rightMotor->setVelocity(rightSpeed);
                    exPos = 1;
                }
                // Sinon on tourne sur nous même pour essayer de toruver un objet rouge
                else{
                    if (exPos==1) {
                        leftMotor->setVelocity(-leftSpeed);
                        rightMotor->setVelocity(rightSpeed);
                    }
                    else {
                        leftMotor->setVelocity(leftSpeed);
                        rightMotor->setVelocity(-rightSpeed);
                    }
                }
            }
        }
    }; 
    
    // Enter here exit cleanup code.
    delete robot;
    return 0;
}