trikset · SeriousCoder · Ju 8000 n 5, 2016 · Jun 6, 2016 · Jun 6, 2016 · Jun 6, 2016
diff --git a/trikControl/include/trikControl/colorSensorInterface.h b/trikControl/include/trikControl/colorSensorInterface.h
@@ -35,11 +35,24 @@ class TRIKCONTROL_EXPORT ColorSensorInterface : public QObject, public DeviceInt
 public slots:
 	/// Initializes a camera.
 	/// @param showOnDisplay - true if we want an image from a camera to be drawn on robot display.
-	virtual void init(bool showOnDisplay) = 0;
+	/// @param returnHSV - true if we want get back hsv color format.
+	virtual void init(bool showOnDisplay, bool returnHSV) = 0;
 
-	/// Returns dominant color in given cell of a grid as a vector [R; G; B] in RGB color scale.
+	/// Returns dominant color in given cell of a grid as a vector [R; G; B] in RGB color scale
+	/// or vector [H; S; V] in HSV color scale.
 	virtual QVector<int> read(int m, int n) = 0;
 
+	/// Returns index of color in given cell of a grid:
+	/// Black   == 0
+	/// Red     == 1
+	/// Yellow  == 2
+	/// Green   == 3
+	/// Cyan    == 4
+	/// Blue    == 5
+	/// Magenta == 6
+	/// White   == 7
+	virtual int getColor(int m, int n) = 0;
+
 	/// Stops detection until init() will be called again.
 	virtual void stop() = 0;
 };

diff --git a/trikControl/src/colorSensor.cpp b/trikControl/src/colorSensor.cpp
@@ -56,9 +56,9 @@ ColorSensor::Status ColorSensor::status() const
 	return mColorSensorWorker->status();
 }
 
-void ColorSensor::init(bool showOnDisplay)
+void ColorSensor::init(bool showOnDisplay, bool returnHSV)
 {
-	QMetaObject::invokeMethod(mColorSensorWorker.data(), "init", Q_ARG(bool, showOnDisplay));
+	QMetaObject::invokeMethod(mColorSensorWorker.data(), "init", Q_ARG(bool, showOnDisplay), Q_ARG(bool, returnHSV));
 }
 
 QVector<int> ColorSensor::read(int m, int n)
@@ -67,6 +67,12 @@ QVector<int> ColorSensor::read(int m, int n)
 	return mColorSensorWorker->read(m, n);
 }
 
+int ColorSensor::getColor(int m, int n)
+{
+	// Read is called synchronously and only takes prepared value from sensor.
+	return mColorSensorWorker->getColor(m, n);
+}
+
 void ColorSensor::stop()
 {
 	QMetaObject::invokeMethod(mColorSensorWorker.data(), "stop");

diff --git a/trikControl/src/colorSensor.h b/trikControl/src/colorSensor.h
@@ -51,10 +51,12 @@ class ColorSensor : public ColorSensorInterface
 	Status status() const override;
 
 public slots:
-	void init(bool showOnDisplay) override;
+	void init(bool showOnDisplay, bool returnHSV) override;
 
 	QVector<int> read(int m, int n) override;
 
+	int getColor(int m, int n) override;
+
 	void stop() override;
 
 private slots:

diff --git a/trikControl/src/colorSensorWorker.cpp b/trikControl/src/colorSensorWorker.cpp
@@ -52,10 +52,15 @@ ColorSensorWorker::~ColorSensorWorker()
 {
 }
 
-void ColorSensorWorker::init(bool showOnDisplay)
+void ColorSensorWorker::init(bool showOnDisplay, bool returnHSV)
 {
 	AbstractVirtualSensorWorker::init();
 	sendCommand(QString("video_out %1").arg(showOnDisplay ? 1 : 0));
+	if (returnHSV)
+	{
+	        sendCommand("hsv");
+	}
+	mReturnHSV = returnHSV;
 }
 
 QVector<int> ColorSensorWorker::read(int m, int n)
@@ -65,7 +70,66 @@ QVector<int> ColorSensorWorker::read(int m, int n)
 		return {-1, -1, -1};
 	}
 
-	return mReading[m - 1][n - 1];
+	return hsvToRgb(mReading[m - 1][n - 1]);
+}
+
+QVector<int> ColorSensorWorker::hsvToRgb(QVector<int> hsv)
+{
+	int H = hsv[0];
+	int S = hsv[1];
+	int V = hsv[2];
+
+	QVector<int> resultRGB({0, 0, 0});
+
+	float r = 0;
+	float g = 0;
+	float b = 0;
+
+	float h = H / 255.0f;
+	float s = S / 255.0f;
+	float v = V / 255.0f;
+
+	v = v < 0.2 ? 0 : v;
+	s = s < 0.2 ? 0 : 1;
+
+	int i = h*6;
+	float f = h*6-i;
+	float p = v * (1 - s);
+	float q = v * (1 - f * s);
+	double t = v * (1 - (1 - f) * s);
+
+	switch(i % 6) {
+		case 0: r = v; g = t; b = p; break;
+		case 1: r = q; g = v; b = p; break;
+		case 2: r = p; g = v; b = t; break;
+		case 3: r = p; g = q; b = v; break;
+		case 4: r = t; g = p; b = v; break;
+		case 5: r = v; g = p; b = q; break;
+	}
+
+	int ri = r*255;
+	int gi = g*255;
+	int bi = b*255;
+
+	resultRGB = {ri, gi, bi};
+
+	return resultRGB;
+}
+
+int ColorSensorWorker::getColor(int m, int n)
+{
+	if (!mReturnHSV) return -1;
+
+	int H = mReading[m - 1][n - 1][0] * 360 / 255;
+	int S = mReading[m - 1][n - 1][1] * 100 / 255;
+	int V = mReading[m - 1][n - 1][2] * 100 / 255;
+
+	int segment = 360 / (COUNT_COLORS - 2);
+
+	if (V > 90 && S < 10) return 7; //return white
+	else if ((V < 15) || (V < 30 && S < 20)) return 0; //return black
+
+	return (((H + segment / 2) / segment) + 1);
 }
 
 QString ColorSensorWorker::sensorName() const
@@ -88,10 +152,10 @@ void ColorSensorWorker::onNewData(const QString &dataLine)
 		for (int i = 0; i < mReadingBuffer.size(); ++i) {
 			for (int j = 0; j < mReadingBuffer[i].size(); ++j) {
 				unsigned const int colorValue = parsedLine[i * mReadingBuffer.size() + j + 1].toUInt();
-				const int r = (colorValue >> 16) & 0xFF;
-				const int g = (colorValue >> 8) & 0xFF;
-				const int b = colorValue & 0xFF;
-				mReadingBuffer[i][j] = {r, g, b};
+				const int C1 = (colorValue >> 16) & 0xFF;
+				const int C2 = (colorValue >> 8) & 0xFF;
+				const int C3 = colorValue & 0xFF;
+				mReadingBuffer[i][j] = {C1, C2, C3};
 			}
 		}
 

diff --git a/trikControl/src/colorSensorWorker.h b/trikControl/src/colorSensorWorker.h
@@ -45,18 +45,46 @@ class ColorSensorWorker : public AbstractVirtualSensorWorker
 public slots:
 	/// Initializes a camera.
 	/// @param showOnDisplay - true if we want an image from a camera to be drawn on robot display.
-	void init(bool showOnDisplay);
+	/// @param returnHSV - true if we want get back hsv color format.
+	void init(bool showOnDisplay, bool returnHSV);
 
-	/// Returns dominant color in given cell of a grid as a vector [R; G; B] in RGB color scale.
+	/// Returns index of color in given cell of a grid:
+	/// Black   == 0
+	/// Red     == 1
+	/// Yellow  == 2
+	/// Green   == 3
+	/// Cyan    == 4
+	/// Blue    == 5
+	/// Magenta == 6
+	/// White   == 7
+	int getColor(int m, int n);
+
+	/// Returns dominant color in given cell of a grid as a vector [R; G; B] in RGB color scale or
+	/// a vector [H, S, V] in HSV color scale.
 	/// If m or n are out of range, returns [-1; -1; -1].
 	/// Can be accessed directly from other thread.
 	QVector<int> read(int m, int n);
 
 private:
+	enum ColorName
+	{
+	Black, // == 0
+	Red,
+	Yellow,
+	Green,
+	Cyan,
+	Blue,
+	Magenta,
+	White, // == 7
+	COUNT_COLORS // count colors
+	};
+
 	QString sensorName() const override;
 
 	void onNewData(const QString &dataLine) override;
 
+	QVector<int> hsvToRgb(QVector<int> hsv);
+
 	/// Current stored reading of a sensor. First two vectors are m*n matrix, inner vector contains 3 values --- red,
 	/// green and blue components of a dominant color in this cell.
 	QVector<QVector<QVector<int>>> mReading;
@@ -66,6 +94,9 @@ public slots:
 
 	/// True, if video stream from camera shall be shown on robot display.
 	bool mShowOnDisplay = true;
+
+	/// True, if need return hsv color format
+	bool mReturnHSV = false;
 };
 
 }