update

src/RobotServoing.cpp

@@ -1,10 +1,11 @@
-#include <chrono>
+#include <chrono>
 #include <thread>
 #include <iostream>
 #include <ctype.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <cmath>
 
 #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)
+bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs) {
-{
-    dxlHandler.enableTorque(false);
-    dxlHandler.closePort();
-}
-
-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
+int main(int argc, char** argv) {
-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<float> 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;
     float fFPS = FPS;
     int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
     int iAreaThresold = AREA_THRESOLD;
 
-    int opt;
+    string sCameraParamFilename = CAM_PARAMS_FILENAME;
-    while ((opt = getopt(argc, argv, ":c:f:s:a:i:")) != -1) {
+    string sColorParamFilename = COLOR_PARAMS_FILENAME;
-        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();
-        }
-    }
 
     initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
 
     int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
-    bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
+    if (!readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV)) return -1;
-    if (!isColorParamsSet) {
-        cout << "[ERROR] Color parameters could not be loaded!" << endl;
-        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 (!cap.read(imgOriginal)) break;
-
-        if (!bSuccess) {
-            cout << "[WARNING] Could not read a new frame from video stream" << endl;
-            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) {
+            float pixelToCmX = 640.0f / 30.0f;
-                line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
+            float pixelToCmY = 480.0f / 22.5f;
-            }
-
-            iLastX = posX;
-            iLastY = posY;
-
-            float pixelToCmX = 640.0f / 60.0f;
-            float pixelToCmY = 480.0f / 45.0f;
             x = (posX - 320.0f) / pixelToCmX;
             y = (240.0f - posY) / pixelToCmY;
+
+            vector<float> 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<float> 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<float> 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
+            continue;
-        }
-
-        // 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
         }
 
         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<float> qi = computeInverseKinematics(x, y);
-        computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
-
-        if (qi.size() == 4) {
-            vector<float> vTargetJointPosition = {qi[0], qi[1], qi[2], qi[3]};
-            _oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
-        }
-
         char key = (char)waitKey(1000.0 / fFPS);
-        if (key == 27) {
+        if (key == 27) break;
-            cout << "[INFO] esc key is pressed by user -> Shutting down!" << endl;
+        if (key == 'u') bIsImageUndistorted = !bIsImageUndistorted;
-            break;
-        }
-        if (key == 'u') {
-            bIsImageUndistorted = !bIsImageUndistorted;
-            cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
-        }
     }
 
     _oDxlHandler.closePort();
     return 0;
-}
+}