Updated esp-master and esp-slave codes

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

@@ -2,13 +2,20 @@
 #include <Wire.h> 
 #include <Arduino.h> 
 #include <esp_now.h> 
-#include <SparkFun_VL53L1X.h> 
+#include <VL53L1X.h>
 #include <Adafruit_MPL3115A2.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)
@@ -36,8 +43,9 @@ 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; 
 int Baro_flag = 0; 
  
 // Define variables to be sent; 
@@ -47,16 +55,16 @@ 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; 
 float Sent_baro = 0; 
  
-int8_t g1 = 0,g2=1,g3=2; 
+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 Barometer variables
 Adafruit_MPL3115A2 baro;
@@ -68,25 +76,26 @@ int LED = LED_BUILTIN;
 // ==================================== data structure ================================================= 
 //Structure the sending data 
 //Must match the receiver structure 
-typedef struct struct_message { 
-  int Rtx; 
-  int Rty; 
-  int Rtz; 
-  int Rrx; 
-  int Rry; 
-  int Rrz; 
-  int Rdist;
+typedef struct struct_message {
+  int Rtx;
+  int Rty;
+  int Rtz;
+  int Rrx;
+  int Rry;
+  int Rrz;
+  int Rdist1;
+  int Rdist2;
   float Rbaro;
-  String DebugM; 
-  int Spwm1; 
-  int Spwm2; 
-  int Spwm3; 
-  int Spwm4; 
-  String Sdir1; 
-  String Sdir2; 
-  String Sdir3; 
-  String Sdir4; 
-} struct_message; 
+  String DebugM;
+  int Spwm1;
+  int Spwm2;
+  int Spwm3;
+  int Spwm4;
+  String Sdir1;
+  String Sdir2;
+  String Sdir3;
+  String Sdir4;
+} struct_message;
 // Create a struct_message to hold incoming sensor readings 
 // struct_message incomingReadings; 
 struct_message receivedData; 
@@ -133,7 +142,7 @@ Camera cam(&interface);
 // ========================== Motor part ==================================== 
 // Create the motor shield object with the default I2C address 
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
-// changed 
+
 Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);  // pwm1
 Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);  // pwm2
 Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);        // pwm3
@@ -152,16 +161,40 @@ void setup() {
   digitalWrite(LED, LOW); 
    
   // -------------- lidar part -------------------- 
-  if (distanceSensor.begin() == 0){ 
-    Serial.println("Lidar online!"); 
-    Lidar_flag = 1; 
-  } else { 
-    Lidar_flag = 0; 
-  } 
+  pinMode(XSHUT_pin1, OUTPUT);
+  pinMode(XSHUT_pin2, OUTPUT);
   
-  distanceSensor.startRanging(); 
-  distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]); 
+  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);
+  
   // ----------------- barometer -----------------
   if (baro.begin()) {
     Serial.println("Barometer online!");
@@ -199,12 +232,11 @@ void setup() {
   } 
   // Register for a callback function that will be called when data is received 
   esp_now_register_recv_cb(OnDataRecv); 
-   
 } 
  
 // ================================== Main loop =================================================== 
-void loop() 
-{  
+void loop()
+{
   //------------------------------------------------------------------------------------- 
   // ========== goal finder ========= 
   int id = -1; 
@@ -230,18 +262,31 @@ void loop()
       Sent_ry = 0; 
       Sent_rz = 0;  
   } 
-  if (Lidar_flag == 1){ 
-    Sent_dist = distanceSensor.getDistance();    // Lidar distance (use the real Lidar data) 
-  } else { 
-    Sent_dist = 0; 
+  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_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; 
+  }  
+  
   if (Baro_flag == 1){ 
     Sent_baro = baro.getAltitude() - groundAlt;  // Barometer altitude (use the real Barometer data) 
   } else {
     Sent_baro = 0.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"; 
@@ -253,7 +298,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; 
   receivedData.Rbaro = Sent_baro; 
   send_var_once(); 
   print_received_Data(); 

Code/Control/Laptop_Code/basic_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;
   float Rbaro;
   String DebugM;
   int Spwm1;
@@ -71,7 +72,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.Rbaro);
   Serial.println(sentData.DebugM);
 }