add two lidar sensors into the slave code and get rid of the openMV cam

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

 #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 
 // MAC Address of the receiver (MASTER)
-uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
+// uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
 //uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
 //uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
 //uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
 //uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
 //uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
@@ -35,9 +42,10 @@ String Rec_dir4;
 int count = 0; 
 int count_var = 0; 
 int print_count = 0; 
-int change_count =0; 
-int Lidar_flag = 0; 
- 
+int change_count =0;  
+int Lidar1_flag = 0; 
+int Lidar2_flag = 0; 
+
 // Define variables to be sent; 
 int Sent_tx = 0; 
 int Sent_ty = 0; 
@@ -46,12 +54,17 @@ 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; 
+// Define Lidar variables 
+VL53L1X Sensor1;
+VL53L1X Sensor2;
+
 int budgetIndex = 4; 
 int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500}; 
  
@@ -68,7 +81,8 @@ typedef struct struct_message {
   int Rrx; 
   int Rry; 
   int Rrz; 
-  int Rdist; 
+  int Rdist1;
+  int Rdist2; 
   String DebugM; 
   int Spwm1; 
   int Spwm2; 
@@ -119,8 +133,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  
 // =============================== All the setup =========================================== 
 //Create the interface that will be used by the camera 
-openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer. 
-openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument 
+// openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer. 
+// openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument 
 //Camera cam(&interface); 
 // ========================== Motor part ==================================== 
 // Create the motor shield object with the default I2C address 
@@ -135,23 +149,46 @@ void setup() {
   // Init Serial Monitor 
   Serial.begin(115200); 
   Wire.begin(); 
-  interface.begin(); //communication between ESP and OpenMV 
+  // interface.begin(); //communication between ESP and OpenMV 
   AFMS.begin();  // create with the default frequency 1.6KHz 
   // -------------- LED part -------------------- 
   pinMode(LED, OUTPUT); 
   digitalWrite(LED, HIGH); 
   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]); 
+  
+ // -------------- lidar part -------------------- 
+  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);
+  Sensor1.setTimeout(500);
+    Sensor1.setDistanceMode(VL53L1X::Medium);
+  Sensor1.setMeasurementTimingBudget(33000);
+  }
+
+
+  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);
+  Sensor2.setTimeout(500);
+  Sensor2.setDistanceMode(VL53L1X::Medium);
+  Sensor2.setMeasurementTimingBudget(33000);
+  }
    
  // --------------------------- esp now --------------------------- 
   // Set device as a Wi-Fi Station 
@@ -211,11 +248,22 @@ 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){ 
     receivedData.DebugM = "found 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();