last lab

include/Kinematics.h

@@ -19,3 +19,6 @@ std::vector<float> computeInverseKinematics(float x, float y);
 std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
 int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
 cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
+
+float getEndEffectorX();
+float getEndEffectorY();

src/Kinematics.cpp

@@ -1,4 +1,7 @@
 #include "Kinematics.h"
+#include <iostream>
+#include <cmath>
+#include <vector>
 
 float deg2rad(float angle)
 {
@@ -29,12 +32,31 @@ std::vector<float> computeInverseKinematics(float x, float y)
 {
     std::vector<float> qi;
 
+//    // Sınır kontrolü (cm cinsinden)
+//    if (x < -11.0f || x > 11.0f) {
+//        std::cout << "[ERROR] Target (" << x << ") is out of allowed range!" << std::endl;
+//        return {};
+//    }
+
+    float r = sqrt(x * x + y * y);
     float q1 = atan2(y, x);
-    float q2 = deg2rad(80.0); // sabit
-    float q3 = acos((sqrt(x*x + y*y) - 5.0 - (6.0 / sqrt((y*y)/(x*x) + 1.0))) / 5.5);
+    float q2 = deg2rad(80.0f); // sabit açı
+
+    float value = (r - 5.0f) / 5.5f;
+
+    // Güvenli değer sınırlandırması:
+    if (value > 1.0f) {
+        std::cout << "[WARNING] Target slightly out of reach. Clamping to 0.999." << std::endl;
+        value = 0.999f;
+    } else if (value < -1.0f) {
+        std::cout << "[ERROR] Target too close or unreachable. Skipping this frame." << std::endl;
+        return {};
+    }
+
+    float q3 = acos(value);
     float q4 = -q1 - q3;
 
-    std::cout << "[INFO] Inverse Kinematics (New): (x, y) -> (q1, q2, q3, q4) = ("
+    std::cout << "[INFO] Inverse Kinematics (Tuned): (x, y) -> (q1, q2, q3, q4) = ("
               << x << ", " << y << ") -> ("
               << rad2deg(q1) << ", " << rad2deg(q2) << ", "
               << rad2deg(q3) << ", " << rad2deg(q4) << ")" << std::endl;

src/RobotServoing.cpp

@@ -21,6 +21,7 @@
 #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;
@@ -72,6 +73,20 @@ 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<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);
@@ -168,8 +183,8 @@ int main(int argc, char** argv)
             iLastX = posX;
             iLastY = posY;
 
-            float pixelToCmX = 640.0f / 30.0f;
-            float pixelToCmY = 480.0f / 22.5f;
+            float pixelToCmX = 640.0f / 60.0f;
+            float pixelToCmY = 480.0f / 45.0f;
             x = (posX - 320.0f) / pixelToCmX;
             y = (240.0f - posY) / pixelToCmY;
         } else {
@@ -180,11 +195,21 @@ int main(int argc, char** argv)
             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
+        }
+
         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 = {deg2rad(0),deg2rad(80),deg2rad(20),deg2rad(90)};
+
         vector<float> qi = computeInverseKinematics(x, y);
         computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
 
@@ -202,7 +227,7 @@ int main(int argc, char** argv)
             bIsImageUndistorted = !bIsImageUndistorted;
             cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
         }
-   }
+    }
 
     _oDxlHandler.closePort();
     return 0;