diff --git a/src/RobotServoing.cpp b/src/RobotServoing.cpp
index a69d87c..cb715d3 100644
--- a/src/RobotServoing.cpp
+++ b/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.yaml"
+#define COLOR_PARAMS_FILENAME "./data/color_params.yaml"
 #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
+#define RATIO_CM_PER_PIXEL_Y 0.1f
 
 using namespace cv;
 using namespace std;
@@ -48,7 +48,6 @@ void closeRobot(DynamixelHandler& dxlHandler)
     dxlHandler.closePort();
 }
 
-// Pikselden dünya koordinatlarına dönüştürme fonksiyonu
 cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height)
 {
     float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y;
@@ -58,18 +57,46 @@ 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 qpen = deg2rad(-90); // in rad
-    std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
-    std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
+    float L  = ROBOT_L;
+    float L4 = ROBOT_L4;
+    float qpen = deg2rad(-90); // rad
     float fFPS = FPS;
-    int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
     int iAreaThresold = AREA_THRESOLD;
 
     initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
 
-    // Kamera açılıyor
+    // === YAML dosyasından renk parametrelerini yükle ===
+    cv::FileStorage fsColorParams(COLOR_PARAMS_FILENAME, cv::FileStorage::READ);
+    if (!fsColorParams.isOpened())
+    {
+        std::cerr << "[ERROR] Renk parametreleri dosyasi acilamadi: " << COLOR_PARAMS_FILENAME << std::endl;
+        return -1;
+    }
+    int lowH, highH, lowS, highS, lowV, highV;
+    fsColorParams["lowH"] >> lowH;
+    fsColorParams["highH"] >> highH;
+    fsColorParams["lowS"] >> lowS;
+    fsColorParams["highS"] >> highS;
+    fsColorParams["lowV"] >> lowV;
+    fsColorParams["highV"] >> highV;
+    fsColorParams.release();
+
+    cv::Scalar lowerHSV(lowH, lowS, lowV);
+    cv::Scalar upperHSV(highH, highS, highV);
+
+    // === Kamera kalibrasyon parametrelerini yükle (XML) ===
+    cv::Mat cameraMatrix, distCoeffs;
+    cv::FileStorage fsCamParams(CAM_PARAMS_FILENAME, cv::FileStorage::READ);
+    if (!fsCamParams.isOpened())
+    {
+        std::cerr << "[ERROR] Kamera parametreleri dosyasi acilamadi: " << CAM_PARAMS_FILENAME << std::endl;
+        return -1;
+    }
+    fsCamParams["camera_matrix"] >> cameraMatrix;
+    fsCamParams["distortion_coefficients"] >> distCoeffs;
+    fsCamParams.release();
+
+    // === Kamera açılıyor ===
     VideoCapture cap(0, cv::CAP_V4L2);
     if (!cap.isOpened())
     {
@@ -77,7 +104,6 @@ int main(int argc, char** argv)
         return -1;
     }
 
-    // İlk görüntü alınıyor
     Mat imgTmp;
     cap.read(imgTmp);
     int img_width = imgTmp.size().width;
@@ -95,19 +121,23 @@ int main(int argc, char** argv)
             break;
         }
 
-        // Görüntüyü HSV formatına çevir
-        cv::Mat imgHSV;
-        cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
+        // === Görüntüyü kalibre et ===
+        cv::Mat imgUndistorted;
+        cv::undistort(imgOriginal, imgUndistorted, cameraMatrix, distCoeffs);
 
-        // Renk eşikleme
+        // Kalibre edilmiş görüntüyü HSV formatına çevir
+        cv::Mat imgHSV;
+        cvtColor(imgUndistorted, imgHSV, cv::COLOR_BGR2HSV);
+
+        // YAML'dan alınan renklerle eşikleme yap
         cv::Mat imgThresholded;
-        inRange(imgHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), imgThresholded);
+        inRange(imgHSV, lowerHSV, upperHSV, imgThresholded);
 
         // Morfolojik işlemler
         cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
         cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
 
-        // Moment hesaplama
+        // Moment hesapla
         Moments oMoments = moments(imgThresholded);
         double dM01 = oMoments.m01;
         double dM10 = oMoments.m10;
@@ -121,24 +151,21 @@ int main(int argc, char** argv)
 
             if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
             {
-                line(imgOriginal, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
+                line(imgUndistorted, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
             }
 
             iLastX = posX;
             iLastY = posY;
         }
 
-        // Piksel koordinatlarını dünya koordinatlarına çevir
         cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height);
         float x = worldCoord.x;
         float y = worldCoord.y;
 
         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)
         {
             std::vector<float> vTargetJointPosition;
@@ -148,20 +175,18 @@ int main(int argc, char** argv)
             _oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
         }
 
-        // Görüntü gösterme
-        imshow("Original", imgOriginal);
 
-        // Kullanıcı girişini kontrol et
+        imshow("Original", imgOriginal);
+        
+
         char key = (char)cv::waitKey(1000.0 / fFPS);
-        if (key == 27) // 'ESC' tuşu çıkışı sağlıyor
+        if (key == 27)
         {
             cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
             break;
         }
     }
 
-    // Robot bağlantısını kapat
     closeRobot(_oDxlHandler);
-
     return 0;
 }