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"
#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 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ı
#define ROBOT_L 15.0f // 1 ve 4. aktüatörler arasındaki toplam uzunluk (cm)
#define ROBOT_L4 5.0f // 4. aktüatörden sonraki uzunluk (cm)
#define RATIO_CM_PER_PIXEL_X 0.1f
#define RATIO_CM_PER_PIXEL_Y 0.1f
using namespace cv;
using namespace std;
@ -48,7 +48,6 @@ void closeRobot(DynamixelHandler& dxlHandler)
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;
@ -58,18 +57,46 @@ cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height)
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 L = ROBOT_L;
float L4 = ROBOT_L4;
float qpen = deg2rad(-90); // rad
float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
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);
if (!cap.isOpened())
{
@ -77,7 +104,6 @@ int main(int argc, char** argv)
return -1;
}
// İlk görüntü alınıyor
Mat imgTmp;
cap.read(imgTmp);
int img_width = imgTmp.size().width;
@ -95,19 +121,23 @@ int main(int argc, char** argv)
break;
}
// Görüntüyü HSV formatına çevir
cv::Mat imgHSV;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
// === Görüntüyü kalibre et ===
cv::Mat imgUndistorted;
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;
inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded);
inRange(imgHSV, lowerHSV, upperHSV, 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
// Moment hesapla
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
@ -121,24 +151,21 @@ int main(int argc, char** argv)
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;
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<float> qi = computeInverseKinematics(x, y, L1, L2);
std::vector<float> qi = computeInverseKinematics(x, y, L, L4);
// Robot eklemleri hareket ettir
if (qi.size() >= 3)
{
std::vector<float> vTargetJointPosition;
@ -148,20 +175,18 @@ int main(int argc, char** argv)
_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);
if (key == 27) // 'ESC' tuşu çıkışı sağlıyor
if (key == 27)
{
cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
break;
}
}
// Robot bağlantısını kapat
closeRobot(_oDxlHandler);
return 0;
}