顏色感測器

顏色感測器會看向地板，讓機器人知道哪裡有坑洞、沼澤和通道。它其實是一台只有一個像素、朝下的攝影機。在入門機器人上，它的名稱是 color，位於機器人前端，這樣就能在輪子到達坑洞之前先看到它。

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;
}

地板顏色

這些數值是用入門機器人在 world2.wbt 上測得的：

地板	紅	綠	藍
一般地板	252	252	252
沼澤	245	222	142
區域 1 與 2 之間的通道（藍色）	91	91	255
區域 2 與 3 之間的通道（紫色）	193	91	251
區域 1 與 4 之間的通道（綠色）	48	255	48
區域 3 與 4 之間的通道（紅色）	255	91	91

警告

數值會隨世界而改變：在 world1.wbt 中，一般地板的讀數大約是 240。請使用範圍而不是精確的數值，並用 Erebus player_controllers 資料夾中的 calibrateColourSensor.py 檢查它們。

坑洞有黑色的邊緣，所以在坑洞前方，地板會先變暗。對這個感測器來說，檢查點看起來幾乎和一般地板一樣，所以請在你的世界中的檢查點上進行校正。

範例：觀察地板

這個程式會讓機器人往前走，每當地板改變時就印出地板的顏色，並在看到坑洞時停下來：

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;
}