From 6023fd5531f77edf90683cb20cffdf06204d544a Mon Sep 17 00:00:00 2001 From: Alperen ISIK Date: Fri, 21 Mar 2025 23:17:11 +0100 Subject: [PATCH] =?UTF-8?q?src/RobotServoing.cpp=20G=C3=BCncelle?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/RobotServoing.cpp | 334 +++++++++++++++++++++--------------------- 1 file changed, 167 insertions(+), 167 deletions(-) diff --git a/src/RobotServoing.cpp b/src/RobotServoing.cpp index 220f9f5..a69d87c 100644 --- a/src/RobotServoing.cpp +++ b/src/RobotServoing.cpp @@ -1,167 +1,167 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#include - -#include "Kinematics.h" -#include "DynamixelHandler.h" - -#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" -#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml" -#define FPS 30.0 -#define STRUCTURAL_ELEMENTS_SIZE 5 -#define AREA_THRESOLD 1000 -#define ROBOT_L1 5 -#define ROBOT_L2 6 -#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı -#define RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı - -using namespace cv; -using namespace std; - -DynamixelHandler _oDxlHandler; -std::string _robotDxlPortName = "/dev/ttyUSB0"; -float _robotDxlProtocol = 2.0; -int _robotDxlBaudRate = 1000000; - -void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) -{ - std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl; - dxlHandler.setDeviceName(portName); - dxlHandler.setProtocolVersion(protocol); - dxlHandler.openPort(); - dxlHandler.setBaudRate(baudRate); - dxlHandler.enableTorque(true); - std::cout << std::endl; -} - -void closeRobot(DynamixelHandler& dxlHandler) -{ - dxlHandler.enableTorque(false); - dxlHandler.closePort(); -} - -// Pikselden dünya koordinatlarına dönüştürme fonksiyonu -cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height) -{ - float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y; - float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X; - return cv::Point2f(x, y); -} - -int main(int argc, char** argv) -{ - float L1 = ROBOT_L1; - float L2 = ROBOT_L2; - float qpen = deg2rad(-90); // in rad - std::string sCameraParamFilename = CAM_PARAMS_FILENAME; - std::string sColorParamFilename = COLOR_PARAMS_FILENAME; - float fFPS = FPS; - int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; - int iAreaThresold = AREA_THRESOLD; - - initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); - - // Kamera açılıyor - VideoCapture cap(0, cv::CAP_V4L2); - if (!cap.isOpened()) - { - cout << "[ERROR] Could not open the camera!" << endl; - return -1; - } - - // İlk görüntü alınıyor - Mat imgTmp; - cap.read(imgTmp); - int img_width = imgTmp.size().width; - int img_height = imgTmp.size().height; - - int iLastX = -1, iLastY = -1; - - while (true) - { - Mat imgOriginal; - bool bSuccess = cap.read(imgOriginal); - if (!bSuccess) - { - cout << "[WARNING] Could not read a new frame from video stream" << endl; - break; - } - - // Görüntüyü HSV formatına çevir - cv::Mat imgHSV; - cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); - - // Renk eşikleme - cv::Mat imgThresholded; - inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded); - - // Morfolojik işlemler - cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); - cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); - - // Moment hesaplama - Moments oMoments = moments(imgThresholded); - double dM01 = oMoments.m01; - double dM10 = oMoments.m10; - double dArea = oMoments.m00; - - int posX = -1, posY = -1; - if (dArea > iAreaThresold) - { - posX = dM10 / dArea; - posY = dM01 / dArea; - - if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) - { - line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2); - } - - iLastX = posX; - iLastY = posY; - } - - // Piksel koordinatlarını dünya koordinatlarına çevir - cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height); - float x = worldCoord.x; - float y = worldCoord.y; - - cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl; - - // Ters kinematik hesaplama - std::vector qi = computeInverseKinematics(x, y, L1, L2); - - // Robot eklemleri hareket ettir - if (qi.size() >= 3) - { - std::vector vTargetJointPosition; - vTargetJointPosition.push_back(qi[1]); - vTargetJointPosition.push_back(qpen); - vTargetJointPosition.push_back(qi[2]); - _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); - } - - // Görüntü gösterme - imshow("Original", imgOriginal); - - // Kullanıcı girişini kontrol et - char key = (char)cv::waitKey(1000.0 / fFPS); - if (key == 27) // 'ESC' tuşu çıkışı sağlıyor - { - cout << "[INFO] ESC key pressed -> Shutting down!" << endl; - break; - } - } - - // Robot bağlantısını kapat - closeRobot(_oDxlHandler); - - return 0; -} +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_data.yaml" +#define FPS 30.0 +#define STRUCTURAL_ELEMENTS_SIZE 5 +#define AREA_THRESOLD 1000 +#define ROBOT_L1 5 +#define ROBOT_L2 6 +#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı +#define RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı + +using namespace cv; +using namespace std; + +DynamixelHandler _oDxlHandler; +std::string _robotDxlPortName = "/dev/ttyUSB0"; +float _robotDxlProtocol = 2.0; +int _robotDxlBaudRate = 1000000; + +void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) +{ + std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl; + dxlHandler.setDeviceName(portName); + dxlHandler.setProtocolVersion(protocol); + dxlHandler.openPort(); + dxlHandler.setBaudRate(baudRate); + dxlHandler.enableTorque(true); + std::cout << std::endl; +} + +void closeRobot(DynamixelHandler& dxlHandler) +{ + dxlHandler.enableTorque(false); + dxlHandler.closePort(); +} + +// Pikselden dünya koordinatlarına dönüştürme fonksiyonu +cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height) +{ + float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y; + float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X; + return cv::Point2f(x, y); +} + +int main(int argc, char** argv) +{ + float L1 = ROBOT_L1; + float L2 = ROBOT_L2; + float qpen = deg2rad(-90); // in rad + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; + float fFPS = FPS; + int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; + int iAreaThresold = AREA_THRESOLD; + + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + // Kamera açılıyor + VideoCapture cap(0, cv::CAP_V4L2); + if (!cap.isOpened()) + { + cout << "[ERROR] Could not open the camera!" << endl; + return -1; + } + + // İlk görüntü alınıyor + Mat imgTmp; + cap.read(imgTmp); + int img_width = imgTmp.size().width; + int img_height = imgTmp.size().height; + + int iLastX = -1, iLastY = -1; + + while (true) + { + Mat imgOriginal; + bool bSuccess = cap.read(imgOriginal); + if (!bSuccess) + { + cout << "[WARNING] Could not read a new frame from video stream" << endl; + break; + } + + // Görüntüyü HSV formatına çevir + cv::Mat imgHSV; + cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); + + // Renk eşikleme + cv::Mat imgThresholded; + inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded); + + // Morfolojik işlemler + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); + cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); + + // Moment hesaplama + Moments oMoments = moments(imgThresholded); + double dM01 = oMoments.m01; + double dM10 = oMoments.m10; + double dArea = oMoments.m00; + + int posX = -1, posY = -1; + if (dArea > iAreaThresold) + { + posX = dM10 / dArea; + posY = dM01 / dArea; + + if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) + { + line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2); + } + + iLastX = posX; + iLastY = posY; + } + + // Piksel koordinatlarını dünya koordinatlarına çevir + cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height); + float x = worldCoord.x; + float y = worldCoord.y; + + cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl; + + // Ters kinematik hesaplama + std::vector qi = computeInverseKinematics(x, y, L1, L2); + + // Robot eklemleri hareket ettir + if (qi.size() >= 3) + { + std::vector vTargetJointPosition; + vTargetJointPosition.push_back(qi[1]); + vTargetJointPosition.push_back(qpen); + vTargetJointPosition.push_back(qi[2]); + _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); + } + + // Görüntü gösterme + imshow("Original", imgOriginal); + + // Kullanıcı girişini kontrol et + char key = (char)cv::waitKey(1000.0 / fFPS); + if (key == 27) // 'ESC' tuşu çıkışı sağlıyor + { + cout << "[INFO] ESC key pressed -> Shutting down!" << endl; + break; + } + } + + // Robot bağlantısını kapat + closeRobot(_oDxlHandler); + + return 0; +}