src/RobotServoing.cpp Güncelle
I changed the servo code according to our situation
This commit is contained in:
parent
65061f49a8
commit
dcad195dea
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue