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:17:11 +01:00
parent 65061f49a8
commit 6023fd5531
1 changed files with 167 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

334
src/RobotServoing.cpp
View File

@ -1,167 +1,167 @@
#include <chrono> #include <chrono>
#include <thread> #include <thread>
#include <iostream> #include <iostream>
#include <ctype.h> #include <ctype.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h> #include <unistd.h>
#include "opencv2/highgui/highgui.hpp" #include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp" #include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp> #include <opencv2/calib3d.hpp>
#include "Kinematics.h" #include "Kinematics.h"
#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.xml" #define COLOR_PARAMS_FILENAME "./data/color_params_data.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_L1 5
#define ROBOT_L2 6 #define ROBOT_L2 6
#define RATIO_CM_PER_PIXEL_X 0.1 // X ekseninde dönüşüm oranı #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 RATIO_CM_PER_PIXEL_Y 0.1 // Y ekseninde dönüşüm oranı
using namespace cv; using namespace cv;
using namespace std; using namespace std;
DynamixelHandler _oDxlHandler; DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = "/dev/ttyUSB0"; std::string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0; float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000; int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
{ {
std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl; std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl;
dxlHandler.setDeviceName(portName); dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol); dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort(); dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate); dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true); dxlHandler.enableTorque(true);
std::cout << std::endl; std::cout << std::endl;
} }
void closeRobot(DynamixelHandler& dxlHandler) void closeRobot(DynamixelHandler& dxlHandler)
{ {
dxlHandler.enableTorque(false); dxlHandler.enableTorque(false);
dxlHandler.closePort(); dxlHandler.closePort();
} }
// Pikselden dünya koordinatlarına dönüştürme fonksiyonu // 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;
float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X; float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X;
return cv::Point2f(x, y); return cv::Point2f(x, y);
} }
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
float L1 = ROBOT_L1; float L1 = ROBOT_L1;
float L2 = ROBOT_L2; float L2 = ROBOT_L2;
float qpen = deg2rad(-90); // in rad float qpen = deg2rad(-90); // in rad
std::string sCameraParamFilename = CAM_PARAMS_FILENAME; std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME; std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
float fFPS = FPS; float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; 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 // Kamera açılıyor
VideoCapture cap(0, cv::CAP_V4L2); VideoCapture cap(0, cv::CAP_V4L2);
if (!cap.isOpened()) if (!cap.isOpened())
{ {
cout << "[ERROR] Could not open the camera!" << endl; cout << "[ERROR] Could not open the camera!" << endl;
return -1; return -1;
} }
// İlk görüntü alınıyor // İ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;
int img_height = imgTmp.size().height; int img_height = imgTmp.size().height;
int iLastX = -1, iLastY = -1; int iLastX = -1, iLastY = -1;
while (true) while (true)
{ {
Mat imgOriginal; Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal); bool bSuccess = cap.read(imgOriginal);
if (!bSuccess) if (!bSuccess)
{ {
cout << "[WARNING] Could not read a new frame from video stream" << endl; cout << "[WARNING] Could not read a new frame from video stream" << endl;
break; break;
} }
// Görüntüyü HSV formatına çevir // Görüntüyü HSV formatına çevir
cv::Mat imgHSV; cv::Mat imgHSV;
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
// Renk eşikleme // Renk eşikleme
cv::Mat imgThresholded; cv::Mat imgThresholded;
inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded); inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), 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 hesaplama
Moments oMoments = moments(imgThresholded); Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01; double dM01 = oMoments.m01;
double dM10 = oMoments.m10; double dM10 = oMoments.m10;
double dArea = oMoments.m00; double dArea = oMoments.m00;
int posX = -1, posY = -1; int posX = -1, posY = -1;
if (dArea > iAreaThresold) if (dArea > iAreaThresold)
{ {
posX = dM10 / dArea; posX = dM10 / dArea;
posY = dM01 / dArea; posY = dM01 / dArea;
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(imgOriginal, 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 // 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 // Ters kinematik hesaplama
std::vector<float> qi = computeInverseKinematics(x, y, L1, L2); std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
// Robot eklemleri hareket ettir // Robot eklemleri hareket ettir
if (qi.size() >= 3) if (qi.size() >= 3)
{ {
std::vector<float> vTargetJointPosition; std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(qi[1]); vTargetJointPosition.push_back(qi[1]);
vTargetJointPosition.push_back(qpen); vTargetJointPosition.push_back(qpen);
vTargetJointPosition.push_back(qi[2]); vTargetJointPosition.push_back(qi[2]);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition); _oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
} }
// Görüntü gösterme // Görüntü gösterme
imshow("Original", imgOriginal); imshow("Original", imgOriginal);
// Kullanıcı girişini kontrol et // Kullanıcı girişini kontrol et
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) // 'ESC' tuşu çıkışı sağlıyor
{ {
cout << "[INFO] ESC key pressed -> Shutting down!" << endl; cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
break; break;
} }
} }
// Robot bağlantısını kapat // Robot bağlantısını kapat
closeRobot(_oDxlHandler); closeRobot(_oDxlHandler);
return 0; return 0;
} }