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

src/RobotServoing.cpp Güncelle #1

Closed
alperen.isik wants to merge 1 commits from alperen.isik-patch-1 into master
pull from: alperen.isik-patch-1
merge into: alperen.isik:master
Conversation 0 Commits 1 Files Changed 1 +167 -167
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 <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#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<float> qi = computeInverseKinematics(x, y, L1, L2);
// Robot eklemleri hareket ettir
if (qi.size() >= 3)
{
std::vector<float> 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 <chrono>
#include <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#include "Kinematics.h"
#include "DynamixelHandler.h"
#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
#define AREA_THRESOLD 1000
#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 // X ekseninde dönüşüm oranı
#define RATIO_CM_PER_PIXEL_Y 0.1f // 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 L = ROBOT_L;
float L4 = ROBOT_L4;
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<float> qi = computeInverseKinematics(x, y, L, L4);
// Robot eklemleri hareket ettir
if (qi.size() >= 3)
{
std::vector<float> 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;
}