RGB Color Detection Using a USB Camera (OpenCV)

In this example, we are going to use a USB camera connected to the VMX-pi to detect basic colors: Red, Green, and Blue. Instead of only displaying the camera feed, the robot will now analyse the image and determine which color is most dominant in a selected area. A camera image is made up of pixels, and each pixel contains three color values:  Red (R)  Green (G)  Blue (B)

By comparing these values, the robot can decide which color it is seeing. For example, if the red value is higher than both green and blue, the robot will identify the color as Red. It is important to understand that OpenCV stores colors in BGR format, not RGB. This means:  pixel[0] = Blue  pixel[1] = Green  pixel[2] = Red This is why the code reads the values in that order.

Constants.h

// Constants.h
#pragma once
namespace constant
{
 static constexpr int CAMERA_INDEX = 0;
 static constexpr int CAMERA_WIDTH = 640;
 static constexpr int CAMERA_HEIGHT = 480;
 // Size of the square sample area in the center of the image
 static constexpr int SAMPLE_BOX_SIZE = 80;
}

Robot.h

#pragma once
#include <frc/TimedRobot.h>
class Robot : public frc::TimedRobot {
public:
 void RobotInit() override;
 void RobotPeriodic() override;
 void DisabledInit() override;
 void DisabledPeriodic() override;
 void AutonomousInit() override;
 void AutonomousPeriodic() override;
 void TeleopInit() override;
 void TeleopPeriodic() override;
 void TestPeriodic() override;
private:

 static void VisionThread();
};

Robot.cpp

// Robot.cpp
#include "Robot.h"
#include "Constants.h"
#include <thread>
#include <string>
#include <cameraserver/CameraServer.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <wpi/raw_ostream.h>
void Robot::VisionThread()
{
#if defined(__linux__)
 cs::UsbCamera camera =
 frc::CameraServer::GetInstance()-
>StartAutomaticCapture(constant::CAMERA_INDEX);
 camera.SetResolution(constant::CAMERA_WIDTH, constant::CAMERA_HEIGHT);
 camera.SetFPS(15);
 cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
 cs::CvSource outputStream =
 frc::CameraServer::GetInstance()->PutVideo(
 "RGB Detection",
 constant::CAMERA_WIDTH,
 constant::CAMERA_HEIGHT
 );
 cv::Mat mat;
 while (true)
 {
 if (cvSink.GrabFrame(mat) == 0)
 {

 outputStream.NotifyError(cvSink.GetError());
 continue;
 }
 int centerX = mat.cols / 2;
 int centerY = mat.rows / 2;
 int halfBox = constant::SAMPLE_BOX_SIZE / 2;
 int startX = centerX - halfBox;
 int startY = centerY - halfBox;
 int endX = centerX + halfBox;
 int endY = centerY + halfBox;
 if (startX < 0) startX = 0;
 if (startY < 0) startY = 0;
 if (endX > mat.cols) endX = mat.cols;
 if (endY > mat.rows) endY = mat.rows;
 long blueTotal = 0;
 long greenTotal = 0;
 long redTotal = 0;
 int pixelCount = 0;
 for (int y = startY; y < endY; y++)
 {
 for (int x = startX; x < endX; x++)
 {
 cv::Vec3b pixel = mat.at<cv::Vec3b>(y, x);
 // OpenCV uses BGR order
 blueTotal += pixel[0];
 greenTotal += pixel[1];
 redTotal += pixel[2];
 pixelCount++;
 }
 }
 int averageBlue = 0;
 int averageGreen = 0;
 int averageRed = 0;
 if (pixelCount > 0)
 {
 averageBlue = static_cast<int>(blueTotal / pixelCount);
 averageGreen = static_cast<int>(greenTotal / pixelCount);

 averageRed = static_cast<int>(redTotal / pixelCount);
 }
 std::string detectedColor = "Unknown";
 if (averageRed > averageGreen && averageRed > averageBlue)
 {
 detectedColor = "Red";
 }
 else if (averageGreen > averageRed && averageGreen > averageBlue)
 {
 detectedColor = "Green";
 }
 else if (averageBlue > averageRed && averageBlue > averageGreen)
 {
 detectedColor = "Blue";
 }
 frc::SmartDashboard::PutNumber("Average Red", averageRed);
 frc::SmartDashboard::PutNumber("Average Green", averageGreen);
 frc::SmartDashboard::PutNumber("Average Blue", averageBlue);
 frc::SmartDashboard::PutString("Detected Color", detectedColor);
 cv::rectangle(
 mat,
 cv::Point(startX, startY),
 cv::Point(endX, endY),
 cv::Scalar(255, 255, 255),
 2
 );
 cv::putText(
 mat,
 detectedColor,
 cv::Point(startX, startY - 10),
 cv::FONT_HERSHEY_SIMPLEX,
 0.8,
 cv::Scalar(255, 255, 255),
 2
 );
 outputStream.PutFrame(mat);
 }
#else
 wpi::errs() << "Vision only available on Linux.\n";

 wpi::errs().flush();
#endif
}
void Robot::RobotInit()
{
#if defined(__linux__)
 std::thread visionThread(VisionThread);
 visionThread.detach();
#else
 wpi::errs() << "Vision only available on Linux.\n";
 wpi::errs().flush();
#endif
}
void Robot::RobotPeriodic() {}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit() {}
void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
 return frc::StartRobot<Robot>();
}
#endif

To see it working on your Smart Dashboard, go to the Camera Server section and select the stream labelled “RGB Detection.” Drag or open RGB Detection, and you will see the live camera feed with the detection box and the identified colour displayed.

Try changing the size of the detection area and observe how it affects the accuracy. Also experiment with different coloured objects under different lighting conditions. Play around with the colour values and test the robot in different environments to make the detection as accurate as possible.