alperen.isik
/
It_Robotics_Pong_Robot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

src/RobotServoing.cpp Güncelle

This commit is contained in:
Alperen ISIK 2025-03-21 23:52:07 +01:00
parent 6023fd5531
commit 94da1729a2
1 changed files with 56 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/RobotServoing.cpp
View File

@ -14,14 +14,14 @@
#include "DynamixelHandler.h" #include "DynamixelHandler.h"
#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" #define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params_data.yaml" #define COLOR_PARAMS_FILENAME "./data/color_params.yaml"
#define FPS 30.0 #define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5 #define STRUCTURAL_ELEMENTS_SIZE 5
#define AREA_THRESOLD 1000 #define AREA_THRESOLD 1000
#define ROBOT_L1 5 #define ROBOT_L 15.0f // 1 ve 4. aktüatörler arasındaki toplam uzunluk (cm)
#define ROBOT_L2 6 #define ROBOT_L4 5.0f // 4. aktüatörden sonraki uzunluk (cm)
#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı #define RATIO_CM_PER_PIXEL_X 0.1f
#define RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı #define RATIO_CM_PER_PIXEL_Y 0.1f
using namespace cv; using namespace cv;
using namespace std; using namespace std;
@ -48,7 +48,6 @@ void closeRobot(DynamixelHandler& dxlHandler)
dxlHandler.closePort(); 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) 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 x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y;
@ -58,18 +57,46 @@ cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height)
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
float L1 = ROBOT_L1; float L = ROBOT_L;
float L2 = ROBOT_L2; float L4 = ROBOT_L4;
float qpen = deg2rad(-90); // in rad float qpen = deg2rad(-90); // rad
std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
float fFPS = FPS; float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD; int iAreaThresold = AREA_THRESOLD;
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// Kamera açılıyor // === YAML dosyasından renk parametrelerini yükle ===
cv::FileStorage fsColorParams(COLOR_PARAMS_FILENAME, cv::FileStorage::READ);
if (!fsColorParams.isOpened())
{
std::cerr << "[ERROR] Renk parametreleri dosyasi acilamadi: " << COLOR_PARAMS_FILENAME << std::endl;
return -1;
}
int lowH, highH, lowS, highS, lowV, highV;
fsColorParams["lowH"] >> lowH;
fsColorParams["highH"] >> highH;
fsColorParams["lowS"] >> lowS;
fsColorParams["highS"] >> highS;
fsColorParams["lowV"] >> lowV;
fsColorParams["highV"] >> highV;
fsColorParams.release();
cv::Scalar lowerHSV(lowH, lowS, lowV);
cv::Scalar upperHSV(highH, highS, highV);
// === Kamera kalibrasyon parametrelerini yükle (XML) ===
cv::Mat cameraMatrix, distCoeffs;
cv::FileStorage fsCamParams(CAM_PARAMS_FILENAME, cv::FileStorage::READ);
if (!fsCamParams.isOpened())
{
std::cerr << "[ERROR] Kamera parametreleri dosyasi acilamadi: " << CAM_PARAMS_FILENAME << std::endl;
return -1;
}
fsCamParams["camera_matrix"] >> cameraMatrix;
fsCamParams["distortion_coefficients"] >> distCoeffs;
fsCamParams.release();
// === Kamera açılıyor ===
VideoCapture cap(0, cv::CAP_V4L2); VideoCapture cap(0, cv::CAP_V4L2);
if (!cap.isOpened()) if (!cap.isOpened())
{ {
@ -77,7 +104,6 @@ int main(int argc, char** argv)
return -1; return -1;
} }
// İlk görüntü alınıyor
Mat imgTmp; Mat imgTmp;
cap.read(imgTmp); cap.read(imgTmp);
int img_width = imgTmp.size().width; int img_width = imgTmp.size().width;
@ -95,19 +121,23 @@ int main(int argc, char** argv)
break; break;
} }
// Görüntüyü HSV formatına çevir // === Görüntüyü kalibre et ===
cv::Mat imgHSV; cv::Mat imgUndistorted;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); cv::undistort(imgOriginal, imgUndistorted, cameraMatrix, distCoeffs);
// Renk eşikleme // Kalibre edilmiş görüntüyü HSV formatına çevir
cv::Mat imgHSV;
cvtColor(imgUndistorted, imgHSV, cv::COLOR_BGR2HSV);
// YAML'dan alınan renklerle eşikleme yap
cv::Mat imgThresholded; cv::Mat imgThresholded;
inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded); inRange(imgHSV, lowerHSV, upperHSV, imgThresholded);
// Morfolojik işlemler // Morfolojik işlemler
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
// Moment hesaplama // Moment hesapla
Moments oMoments = moments(imgThresholded); Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01; double dM01 = oMoments.m01;
double dM10 = oMoments.m10; double dM10 = oMoments.m10;
@ -121,24 +151,21 @@ int main(int argc, char** argv)
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{ {
line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2); line(imgUndistorted, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
} }
iLastX = posX; iLastX = posX;
iLastY = posY; iLastY = posY;
} }
// Piksel koordinatlarını dünya koordinatlarına çevir
cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height); cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height);
float x = worldCoord.x; float x = worldCoord.x;
float y = worldCoord.y; float y = worldCoord.y;
cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl; cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl;
// Ters kinematik hesaplama std::vector<float> qi = computeInverseKinematics(x, y, L, L4);
std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
// Robot eklemleri hareket ettir
if (qi.size() >= 3) if (qi.size() >= 3)
{ {
std::vector<float> vTargetJointPosition; std::vector<float> vTargetJointPosition;
@ -148,20 +175,18 @@ int main(int argc, char** argv)
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition); _oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
} }
// Görüntü gösterme
imshow("Original", imgOriginal);
// Kullanıcı girişini kontrol et imshow("Original", imgOriginal);
char key = (char)cv::waitKey(1000.0 / fFPS); char key = (char)cv::waitKey(1000.0 / fFPS);
if (key == 27) // 'ESC' tuşu çıkışı sağlıyor if (key == 27)
{ {
cout << "[INFO] ESC key pressed -> Shutting down!" << endl; cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
break; break;
} }
} }
// Robot bağlantısını kapat
closeRobot(_oDxlHandler); closeRobot(_oDxlHandler);
return 0; return 0;
} }