src/RobotServoing.cpp Güncelle

I changed the servo code according to our situation

src/RobotServoing.cpp

@@ -14,14 +14,14 @@
 #include "DynamixelHandler.h"
 
 #define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml"
-#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml"
+#define COLOR_PARAMS_FILENAME "./data/color_params.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ı
+#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;
@@ -58,8 +58,8 @@ cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height)
 
 int main(int argc, char** argv)
 {
-    float L1 = ROBOT_L1;
-    float L2 = ROBOT_L2;
+    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;
@@ -136,7 +136,7 @@ int main(int argc, char** argv)
         cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl;
 
         // Ters kinematik hesaplama
-        std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
+        std::vector<float> qi = computeInverseKinematics(x, y, L, L4);
 
         // Robot eklemleri hareket ettir
         if (qi.size() >= 3)