From cd2130a571e0a6bbeda51d542cb806d32a9eb558 Mon Sep 17 00:00:00 2001 From: Hasan Emre AYDIN Date: Tue, 22 Apr 2025 20:16:46 +0200 Subject: [PATCH] update --- src/RobotServoing.cpp | 166 ++++++++++++++++-------------------------- 1 file changed, 61 insertions(+), 105 deletions(-) diff --git a/src/RobotServoing.cpp b/src/RobotServoing.cpp index 2564ad8..8eb0df1 100644 --- a/src/RobotServoing.cpp +++ b/src/RobotServoing.cpp @@ -1,10 +1,11 @@ -#include +#include #include #include #include #include #include #include +#include #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" @@ -21,7 +22,6 @@ #define ROBOT_L1 5 #define ROBOT_L2 5.5 #define ROBOT_L3 6 -#define PUSH_THRESHOLD_CM 4.0f // İtme eşiği (cm) using namespace std; using namespace cv; @@ -31,8 +31,35 @@ string _robotDxlPortName = "/dev/ttyUSB0"; float _robotDxlProtocol = 2.0; int _robotDxlBaudRate = 1000000; -void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, int baudRate) -{ +struct Vec2 { + double x; + double y; +}; + +Vec2 computePushTarget(const Vec2& ee_pos, const Vec2& ball_pos) { + Vec2 direction = { + ball_pos.x - ee_pos.x, + ball_pos.y - ee_pos.y + }; + + double length = std::sqrt(direction.x * direction.x + direction.y * direction.y); + if (length == 0) return ee_pos; + + Vec2 unit_dir = { + direction.x / length, + direction.y / length + }; + + double push_distance = 0.05; + Vec2 push_target = { + ball_pos.x + unit_dir.x * push_distance, + ball_pos.y + unit_dir.y * push_distance + }; + + return push_target; +} + +void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, int baudRate) { cout << "===Initialization of the Dynamixel Motor communication====" << endl; dxlHandler.setDeviceName(portName); dxlHandler.setProtocolVersion(protocol); @@ -42,14 +69,7 @@ void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, in cout << endl; } -void closeRobot(DynamixelHandler& dxlHandler) -{ - dxlHandler.enableTorque(false); - dxlHandler.closePort(); -} - -bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs) -{ +bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs) { FileStorage fs(filename, FileStorage::READ); if (!fs.isOpened()) { cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !" << endl; @@ -60,8 +80,7 @@ bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs) return true; } -bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) -{ +bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) { FileStorage fs(filename, FileStorage::READ); if (!fs.isOpened()) { cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !" << endl; @@ -73,60 +92,21 @@ bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, i return true; } -// Yeni fonksiyon: Rastgele pozisyon göndererek topu itmek -void pushBallRandomly(DynamixelHandler &dxlHandler) { - float randAngle1 = deg2rad(rand() % 90 - 45); // -45 ile 45 arasında rastgele açı - float randAngle2 = deg2rad(rand() % 30 + 60); // 60 ile 90 arasında rastgele açı (kol havada kalmalı) - float randAngle3 = deg2rad(rand() % 90 - 45); - float randAngle4 = deg2rad(rand() % 180 - 90); - - vector qi = {randAngle1, randAngle2, randAngle3, randAngle4}; - computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]); - dxlHandler.sendTargetJointPosition(qi); - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); // biraz bekle -} - -int main(int argc, char** argv) -{ - float q2 = deg2rad(80); - string sCameraParamFilename = CAM_PARAMS_FILENAME; - string sColorParamFilename = COLOR_PARAMS_FILENAME; +int main(int argc, char** argv) { float fFPS = FPS; int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; int iAreaThresold = AREA_THRESOLD; - int opt; - while ((opt = getopt(argc, argv, ":c:f:s:a:i:")) != -1) { - switch (opt) { - case 'c': sColorParamFilename = optarg; break; - case 'f': fFPS = atof(optarg); break; - case 's': iStructuralElementSize = atoi(optarg); break; - case 'a': iAreaThresold = atoi(optarg); break; - case 'i': sCameraParamFilename = optarg; break; - case '?': - fprintf(stderr, "Unknown option -%c.\n", optopt); - return 1; - default: - abort(); - } - } + string sCameraParamFilename = CAM_PARAMS_FILENAME; + string sColorParamFilename = COLOR_PARAMS_FILENAME; initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV; - bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); - if (!isColorParamsSet) { - cout << "[ERROR] Color parameters could not be loaded!" << endl; - return -1; - } + if (!readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV)) return -1; - bool bIsImageUndistorted = true; Mat cameraMatrix, distCoeffs; bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs); - if (!isCamParamsSet) { - cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << endl; - } VideoCapture cap(0, cv::CAP_V4L2); if (!cap.isOpened()) { @@ -137,26 +117,20 @@ int main(int argc, char** argv) int iLastX = -1, iLastY = -1; Mat imgTmp; cap.read(imgTmp); + bool bIsImageUndistorted = true; while (true) { Mat imgLines = Mat::zeros(imgTmp.size(), CV_8UC3); Mat imgOriginal; - bool bSuccess = cap.read(imgOriginal); - - if (!bSuccess) { - cout << "[WARNING] Could not read a new frame from video stream" << endl; - break; - } + if (!cap.read(imgOriginal)) break; if (bIsImageUndistorted && isCamParamsSet) { Mat temp = imgOriginal.clone(); undistort(temp, imgOriginal, cameraMatrix, distCoeffs); } - Mat imgHSV; + Mat imgHSV, imgThresholded; cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); - - Mat imgThresholded; inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize))); @@ -176,59 +150,41 @@ int main(int argc, char** argv) posX = dM10 / dArea; posY = dM01 / dArea; - if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) { - line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2); - } - - iLastX = posX; - iLastY = posY; - - float pixelToCmX = 640.0f / 60.0f; - float pixelToCmY = 480.0f / 45.0f; + float pixelToCmX = 640.0f / 30.0f; + float pixelToCmY = 480.0f / 22.5f; x = (posX - 320.0f) / pixelToCmX; y = (240.0f - posY) / pixelToCmY; + + vector current_q = {deg2rad(0), deg2rad(80), deg2rad(0), deg2rad(0)}; + computeForwardKinematics(current_q[0], current_q[1], current_q[2], current_q[3]); + + Vec2 ee_pos = { getEndEffectorX(), getEndEffectorY() }; // Burada robotun ucu pozisyonu + Vec2 ball_pos = { x, y }; + Vec2 push_target = computePushTarget(ee_pos, ball_pos); + + vector qi = computeInverseKinematics(push_target.x, push_target.y); + computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]); + + if (qi.size() == 4) { + _oDxlHandler.sendTargetJointPosition(qi); + } + } else { - // Top görünmüyorsa başlangıç pozisyonuna dön vector qi = {deg2rad(0), deg2rad(80), deg2rad(0), deg2rad(0)}; computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]); _oDxlHandler.sendTargetJointPosition(qi); - continue; // bu frame'i geç, aşağıdaki inverse kinematic vs. çalışmasın - } - - // Yeşil çizgi ekle - line(imgOriginal, Point(0, imgOriginal.rows / 2 + PUSH_THRESHOLD_CM * (480.0f / 22.5f)), - Point(imgOriginal.cols, imgOriginal.rows / 2 + PUSH_THRESHOLD_CM * (480.0f / 22.5f)), - Scalar(0, 255, 0), 2); - - if (y < PUSH_THRESHOLD_CM) { // top çizgiye yaklaştı mı kontrolü - pushBallRandomly(_oDxlHandler); - continue; // itme sonrası bu frame'i atla + continue; } imshow("Thresholded Image", imgThresholded); drawMarker(imgOriginal, Point(imgTmp.size().width / 2, imgTmp.size().height / 2), 10, MARKER_CROSS, LINE_8); - imgOriginal = imgOriginal + imgLines; imshow("Original", imgOriginal); - vector qi = computeInverseKinematics(x, y); - computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]); - - if (qi.size() == 4) { - vector vTargetJointPosition = {qi[0], qi[1], qi[2], qi[3]}; - _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); - } - char key = (char)waitKey(1000.0 / fFPS); - if (key == 27) { - cout << "[INFO] esc key is pressed by user -> Shutting down!" << endl; - break; - } - if (key == 'u') { - bIsImageUndistorted = !bIsImageUndistorted; - cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl; - } + if (key == 27) break; + if (key == 'u') bIsImageUndistorted = !bIsImageUndistorted; } _oDxlHandler.closePort(); return 0; -} +} \ No newline at end of file