Robots suiveurs 3 - Code RobotCamera
Révision datée du 12 mai 2020 à 16:22 par Clément Luton (discussion | contributions)
<nowiki>
- 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 sense 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, vert et bleu des pixels pour chacune des 3 parties de l'image etudié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 rapporhce 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{
std::cout<<"redg : "<< redg <<"redd : "<< redd << "red : "<< red <<std::endl;
std::cout<<"blueg : "<< blueg <<"blued : "<< blued << "blue : "<< blue <<std::endl;
std::cout<<"greeng : "<< greeng <<"greend : "<< greend << "green : "<< green <<std::endl;
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 toruver 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;
}
<\nowiki>
