Lab5

include/DynamixelHandler.h

@@ -31,9 +31,9 @@
 
 // rotation direction
 #define ROT_DIRECTION_Q1 1
-#define ROT_DIRECTION_Q2 1
-#define ROT_DIRECTION_Q3 1
-#define ROT_DIRECTION_Q4 1
+#define ROT_DIRECTION_Q2 -1
+#define ROT_DIRECTION_Q3 -1
+#define ROT_DIRECTION_Q4 -1
 
 // nb of joints
 #define NB_JOINTS 4

src/Kinematics.cpp

@@ -14,7 +14,7 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float
 {
     float x = 6*cos(q1)*cos(q3)*cos(q4) - 6*cos(q1)*sin(q3)*sin(q4) + 5.5*cos(q1)*cos(q3) + 5*cos(q1);
     float y = 6*sin(q1)*cos(q3)*cos(q4) - 6*sin(q1)*sin(q3)*sin(q4) + 5.5*sin(q1)*cos(q3) + 5*sin(q1);
-    float z = 6*sin(q3)*cos(q4) + 6*cos(q3)*sin(q4) + 5.5*sin(q3);
+    float z = 0;
 
     std::cout << "[INFO] Forward Kinematics : (q1, q2, q3, q4)->(x, y, z) = ("
               << rad2deg(q1) << ", " << rad2deg(q2) << ", "
@@ -30,7 +30,7 @@ std::vector<float> computeInverseKinematics(float x, float y)
     std::vector<float> qi;
 
     float q1 = atan2(y, x);
-    float q2 = deg2rad(-90.0); // sabit
+    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 q4 = -q1 - q3;
 

src/RobotServoing.cpp

@@ -1,4 +1,4 @@
-#include <chrono>
+#include <chrono>
 #include <thread>
 #include <iostream>
 #include <ctype.h>
@@ -74,7 +74,7 @@ bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, i
 
 int main(int argc, char** argv)
 {
-    float q2 = deg2rad(90);
+    float q2 = deg2rad(80);
     string sCameraParamFilename = CAM_PARAMS_FILENAME;
     string sColorParamFilename = COLOR_PARAMS_FILENAME;
     float fFPS = FPS;
@@ -168,15 +168,23 @@ int main(int argc, char** argv)
             iLastX = posX;
             iLastY = posY;
 
-            x = 10.0; // Örnek değer
-            y = 10.0;
+            float pixelToCmX = 640.0f / 30.0f;
+            float pixelToCmY = 480.0f / 22.5f;
+            x = (posX - 320.0f) / pixelToCmX;
+            y = (240.0f - posY) / pixelToCmY;
+        } 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
         }
 
         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]);
 
@@ -194,7 +202,7 @@ int main(int argc, char** argv)
             bIsImageUndistorted = !bIsImageUndistorted;
             cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
         }
-    }
+   }
 
     _oDxlHandler.closePort();
     return 0;