Sensor de cor

O sensor de cor olha para o piso, então ele informa o robô sobre buracos, pântanos e passagens. Ele é uma câmera de um único pixel, apontada para baixo. No robô inicial, ele se chama color e fica na frente do robô, para que veja um buraco antes que as rodas cheguem até ele.

Python

from controller import Robot

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

colorSensor = robot.getDevice("color")
colorSensor.enable(timeStep)

while robot.step(timeStep) != -1:
    image = colorSensor.getImage()  # A single pixel
    r = colorSensor.imageGetRed(image, 1, 0, 0)
    g = colorSensor.imageGetGreen(image, 1, 0, 0)
    b = colorSensor.imageGetBlue(image, 1, 0, 0)
    print("r:", r, "g:", g, "b:", b)

C/C++

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

using namespace webots;
using namespace std;

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  Camera *colorSensor = robot->getCamera("color");
  colorSensor->enable(timeStep);

  while (robot->step(timeStep) != -1) {
    const unsigned char *image = colorSensor->getImage(); // A single pixel
    int r = Camera::imageGetRed(image, 1, 0, 0);
    int g = Camera::imageGetGreen(image, 1, 0, 0);
    int b = Camera::imageGetBlue(image, 1, 0, 0);
    cout << "r: " << r << " g: " << g << " b: " << b << endl;
  }

  delete robot;
  return 0;
}

Cores do piso

Estes valores foram medidos com o robô inicial em world2.wbt:

Piso	Vermelho	Verde	Azul
Normal	252	252	252
Pântano	245	222	142
Passagem entre as áreas 1 e 2 (azul)	91	91	255
Passagem entre as áreas 2 e 3 (roxa)	193	91	251
Passagem entre as áreas 1 e 4 (verde)	48	255	48
Passagem entre as áreas 3 e 4 (vermelha)	255	91	91

Cuidado

Os valores mudam de um mundo para outro: o piso normal fica em cerca de 240 em world1.wbt. Use faixas em vez de valores exatos e confira-as com calibrateColourSensor.py, na pasta player_controllers do Erebus.

Os buracos têm uma borda preta, então o piso escurece logo antes de um buraco. Os pontos de controle parecem quase um piso normal para este sensor, então faça a calibração em um deles no seu mundo.

Exemplo: observando o piso

Este programa anda para a frente, mostra o piso toda vez que ele muda e para se vir um buraco:

Python

from controller import Robot

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

colorSensor = robot.getDevice("color")
colorSensor.enable(timeStep)
leftWheel = robot.getDevice("left wheel motor")
rightWheel = robot.getDevice("right wheel motor")
leftWheel.setPosition(float("inf"))
rightWheel.setPosition(float("inf"))

def floorType(r, g, b):
    # Tune these ranges for your own robot and worlds
    brightness = (r + g + b) / 3
    if brightness < 80:
        return "hole"
    if r > 220 and g > 200 and b < 190:
        return "swamp"
    if b > 220 and g < 130:
        return "purple passage" if r > 150 else "blue passage"
    if r > 220 and g < 130 and b < 130:
        return "red passage"
    if g > 220 and r < 100 and b < 100:
        return "green passage"
    return "floor"

leftWheel.setVelocity(3.0)
rightWheel.setVelocity(3.0)
last = None

while robot.step(timeStep) != -1:
    image = colorSensor.getImage()
    floor = floorType(colorSensor.imageGetRed(image, 1, 0, 0),
                      colorSensor.imageGetGreen(image, 1, 0, 0),
                      colorSensor.imageGetBlue(image, 1, 0, 0))
    if floor != last:
        print("Floor:", floor)
        last = floor
    if floor == "hole":  # Stop before falling in
        leftWheel.setVelocity(0)
        rightWheel.setVelocity(0)
        break

C/C++

#include <iostream>
#include <cmath>
#include <string>
#include <webots/Robot.hpp>
#include <webots/Camera.hpp>
#include <webots/Motor.hpp>

using namespace webots;
using namespace std;

// Tune these ranges for your own robot and worlds
string floorType(int r, int g, int b) {
  int brightness = (r + g + b) / 3;
  if (brightness < 80)
    return "hole";
  if (r > 220 && g > 200 && b < 190)
    return "swamp";
  if (b > 220 && g < 130)
    return r > 150 ? "purple passage" : "blue passage";
  if (r > 220 && g < 130 && b < 130)
    return "red passage";
  if (g > 220 && r < 100 && b < 100)
    return "green passage";
  return "floor";
}

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  Camera *colorSensor = robot->getCamera("color");
  colorSensor->enable(timeStep);
  Motor *leftWheel = robot->getMotor("left wheel motor");
  Motor *rightWheel = robot->getMotor("right wheel motor");
  leftWheel->setPosition(INFINITY);
  rightWheel->setPosition(INFINITY);

  leftWheel->setVelocity(3.0);
  rightWheel->setVelocity(3.0);
  string last;

  while (robot->step(timeStep) != -1) {
    const unsigned char *image = colorSensor->getImage();
    string floor = floorType(Camera::imageGetRed(image, 1, 0, 0), Camera::imageGetGreen(image, 1, 0, 0),
                             Camera::imageGetBlue(image, 1, 0, 0));
    if (floor != last) {
      cout << "Floor: " << floor << endl;
      last = floor;
    }
    if (floor == "hole") { // Stop before falling in
      leftWheel->setVelocity(0);
      rightWheel->setVelocity(0);
      break;
    }
  }

  delete robot;
  return 0;
}