diff --git a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino
index 446b2506f8497ee6d91e1e8a121d0b4f0ffc28cb..957a550939007be375f9ce8bb5020d8825e336fe 100644
--- a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino
+++ b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino
@@ -29,7 +29,8 @@ typedef struct struct_message {
   int Rrx;
   int Rry;
   int Rrz;
-  int Rdist;
+  int Rdist1;
+  int Rdist2;
   String DebugM;
   int Spwm1;
   int Spwm2;
@@ -70,7 +71,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
   Serial.println(sentData.Rrx);
   Serial.println(sentData.Rry);
   Serial.println(sentData.Rrz);
-  Serial.println(sentData.Rdist);
+  Serial.println(sentData.Rdist1);
+  Serial.println(sentData.Rdist2);
   Serial.println(sentData.DebugM);
 }
 // =================================== setup ==================================================
diff --git a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
index c32e3b41b30ed8d5dc0e58491546bf190020a22c..38d067a1ceefe260a08556b550f00187534ba91d 100644
--- a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
@@ -1,12 +1,21 @@
 #include <esp_now.h> 
 #include <WiFi.h> 
 #include <Wire.h> 
-#include <SparkFun_VL53L1X.h> 
+#include <VL53L1X.h>
 #include <Arduino.h> 
  
 #include "Camera.h" 
 #include "utilities.h" 
 #include <Adafruit_MotorShield.h> 
+
+
+// For LIDAR 1 and 2
+#define XSHUT_pin1 A7
+#define XSHUT_pin2 A6
+
+#define Sensor1_newAddress 43 
+#define Sensor2_newAddress 45
+
  
 // REPLACE WITH THE MAC Address of your receiver (MASTER) 
 // Slave: 40:F5:20:44:B6:4C 
@@ -36,7 +45,8 @@ int count = 0;
 int count_var = 0; 
 int print_count = 0; 
 int change_count =0; 
-int Lidar_flag = 0; 
+int Lidar1_flag = 0; 
+int Lidar2_flag = 0; 
  
 // Define variables to be sent; 
 int Sent_tx = 0; 
@@ -45,15 +55,15 @@ int Sent_tz = 0;
 int Sent_rx = 0; 
 int Sent_ry = 0; 
 int Sent_rz = 0; 
-int Sent_dist = 0; 
+int Sent_dist1 = 0; 
+int Sent_dist2 = 0; 
  
 int8_t g1 = 0,g2=1,g3=2; 
 int8_t goal_id[3] = {g1, g2, g3}; 
  
 // Define Lidar variables 
-SFEVL53L1X distanceSensor; 
-int budgetIndex = 4; 
-int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500}; 
+VL53L1X Sensor1;
+VL53L1X Sensor2;
  
 // Define LED variable 
 int LED = LED_BUILTIN; 
@@ -68,7 +78,8 @@ typedef struct struct_message {
   int Rrx; 
   int Rry; 
   int Rrz; 
-  int Rdist; 
+  int Rdist1;
+  int Rdist2; 
   String DebugM; 
   int Spwm1; 
   int Spwm2; 
@@ -143,15 +154,41 @@ void setup() {
   digitalWrite(LED, LOW); 
    
   // -------------- lidar part -------------------- 
-  if (distanceSensor.begin() == 0){ 
-    Serial.println("Sensor online!"); 
-    Lidar_flag = 1; 
-  }else { 
-    Lidar_flag = 0; 
-  } 
-      
-  distanceSensor.startRanging(); 
-  distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]); 
+
+  pinMode(XSHUT_pin1, OUTPUT);
+  pinMode(XSHUT_pin2, OUTPUT);
+  
+  Wire.setClock(100000); 
+
+  pinMode(XSHUT_pin1, INPUT);
+  if (!Sensor1.init()){
+    Serial.println("Failed to detect and initialize sensor1!");
+    Lidar1_flag = 0; 
+  } else {
+    Lidar1_flag = 1; 
+  }
+  delay(10);
+  Sensor1.setAddress(Sensor1_newAddress);
+
+  pinMode(XSHUT_pin2, INPUT);
+  if (!Sensor2.init())
+  {
+    Serial.println("Failed to detect and initialize sensor2!");
+    Lidar2_flag = 0; 
+  } else {
+    Lidar2_flag = 1; 
+  }
+  delay(10);
+  Sensor2.setAddress(Sensor2_newAddress);
+  
+  Sensor1.setTimeout(500);
+  Sensor2.setTimeout(500);
+
+  Sensor1.setDistanceMode(VL53L1X::Medium);
+  Sensor1.setMeasurementTimingBudget(33000);
+  Sensor2.setDistanceMode(VL53L1X::Medium);
+  Sensor2.setMeasurementTimingBudget(33000);
+  
    
  // --------------------------- esp now --------------------------- 
   // Set device as a Wi-Fi Station 
@@ -211,13 +248,24 @@ void loop()
       Sent_ry = 0; 
       Sent_rz = 0;  
   } 
-  if (Lidar_flag == 1){ 
-    Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data) 
+  if (Lidar1_flag == 1){ 
+    Sensor1.startContinuous(50);
+    Sensor1.read();
+    Sensor1.stopContinuous();
+    Sent_dist1 = Sensor1.ranging_data.range_mm; // Lidar distance (use the real Lidar data) 
   }else { 
-    Sent_dist = 0; 
-  } 
+    Sent_dist1 = 0; 
+  }
+  if (Lidar2_flag == 1){ 
+    Sensor2.startContinuous(50);
+    Sensor2.read();
+    Sensor2.stopContinuous();
+    Sent_dist2 = Sensor2.ranging_data.range_mm; // Lidar distance (use the real Lidar data) 
+  }else { 
+    Sent_dist2 = 0; 
+  }  
   // ========== lidar state info ========= 
-  if (Sent_dist < 300 && Sent_dist > 0){ 
+  if (Sent_dist1 < 300 && Sent_dist1 > 0){ 
     receivedData.DebugM = "found b"; 
   }else{ 
     receivedData.DebugM = "no b"; 
@@ -229,7 +277,8 @@ void loop()
   receivedData.Rrx = Sent_rx;   
   receivedData.Rry = Sent_ry;  
   receivedData.Rrz = Sent_rz; 
-  receivedData.Rdist = Sent_dist; 
+  receivedData.Rdist1 = Sent_dist1;
+  receivedData.Rdist2 = Sent_dist2; 
   send_var_once(); 
   print_received_Data(); 
    
@@ -238,6 +287,7 @@ void loop()
   //------------------------------------------------------------------------------------- 
 } 
 // ================================== ^ Main loop ^ =================================================== 
+
 void control_motion(){ 
   // vertical motor 
   motorVertical_L->setSpeed(abs(Rec_pwm1));