add the slave from other branch

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

+#include <esp_now.h> 
+#include <WiFi.h> 
+#include <Wire.h> 
+#include <SparkFun_VL53L1X.h> 
+#include <Arduino.h> 
+ 
+#include "Camera.h" 
+#include "utilities.h" 
+#include <Adafruit_MotorShield.h> 
+ 
+// 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[] = {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, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
+String success; 
+ 
+// Define variables to store incoming readings 
+String sentDebugM = ""; 
+int Rec_pwm1; 
+int Rec_pwm2; 
+int Rec_pwm3; 
+int Rec_pwm4; 
+String Rec_dir1; 
+String Rec_dir2; 
+String Rec_dir3; 
+String Rec_dir4; 
+ 
+int count = 0; 
+int count_var = 0; 
+int print_count = 0; 
+int change_count =0; 
+int Lidar_flag = 0; 
+ 
+// Define variables to be sent; 
+int Sent_tx = 0; 
+int Sent_ty = 0; 
+int Sent_tz = 0; 
+int Sent_rx = 0; 
+int Sent_ry = 0; 
+int Sent_rz = 0; 
+int Sent_dist = 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}; 
+ 
+// Define LED variable 
+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; 
+  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; 
+ 
+// =================================== send and received function ===================================== 
+// Callback when data is sent 
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { 
+  Serial.print("\r\nLast Packet Send Status:\t"); 
+  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail"); 
+  if (status ==0){ 
+    success = "Delivery Success 🙂"; 
+  } 
+  else{ 
+    success = "Delivery Fail 🙁"; 
+  } 
+} 
+ 
+// Callback when data is received 
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { 
+  memcpy(&receivedData, incomingData, sizeof(receivedData)); 
+  // the data format: 225 225 225 225 + + - - 
+  // + up / forward  
+  // - down / backward 
+  Rec_pwm1 = receivedData.Spwm1; 
+  Rec_pwm2 = receivedData.Spwm2;  
+  Rec_pwm3 = receivedData.Spwm3; 
+  Rec_pwm4 = receivedData.Spwm4;     
+  Rec_dir1 = receivedData.Sdir1;   
+  Rec_dir2 = receivedData.Sdir2;  
+  Rec_dir3 = receivedData.Sdir3;  
+  Rec_dir4 = receivedData.Sdir4;    
+  count_var = 0; 
+  count = 0; 
+  print_count=0; 
+  change_count = 0;   
+   change_count = 0;   
+} 
+ 
+// =============================== 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 
+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 
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);       // pwm4  
+// ==================================== Set up ================================================= 
+void setup() { 
+  // Init Serial Monitor 
+  Serial.begin(115200); 
+  Wire.begin(); 
+  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]); 
+   
+ // --------------------------- esp now --------------------------- 
+  // Set device as a Wi-Fi Station 
+  WiFi.mode(WIFI_STA); 
+ 
+  // Init ESP-NOW 
+  if (esp_now_init() != ESP_OK) { 
+    Serial.println("Error initializing ESP-NOW"); 
+    return; 
+  } 
+   
+  // Once ESPNow is successfully Init, we will register for Send CB to 
+  // get the status of Trasnmitted packet 
+  esp_now_register_send_cb(OnDataSent); 
+   
+  // Register peer 
+  esp_now_peer_info_t peerInfo; 
+  memcpy(peerInfo.peer_addr, broadcastAddress, 6); 
+  peerInfo.channel = 0;   
+  peerInfo.encrypt = false; 
+   
+  // Add peer         
+  if (esp_now_add_peer(&peerInfo) != ESP_OK){ 
+    Serial.println("Failed to add peer"); 
+    return; 
+  } 
+  // Register for a callback function that will be called when data is received 
+  esp_now_register_recv_cb(OnDataRecv); 
+   
+} 
+ 
+// ================================== Main loop =================================================== 
+void loop() 
+{  
+  //------------------------------------------------------------------------------------- 
+  // ========== goal finder ========= 
+  int id = -1; 
+  int tx = 0; int ty = 0; int tz = 0; 
+  int rx = 0; int ry = 0; int rz = 0; 
+  int x = 0; 
+  int y = 0;  
+  bool goalfind_flag = 0; 
+   
+  goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz); 
+  if (goalfind_flag){ 
+      Sent_tx = tx; 
+      Sent_ty = ty; 
+      Sent_tz = tz; // cm 
+      Sent_rx = rx; 
+      Sent_ry = ry; 
+      Sent_rz = rz;  
+  }else{ 
+      Sent_tx = 0; 
+      Sent_ty = 0; 
+      Sent_tz = 0; // cm 
+      Sent_rx = 0; 
+      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; 
+  } 
+  // ========== lidar state info ========= 
+  if (Sent_dist < 300 && Sent_dist > 0){ 
+    receivedData.DebugM = "found b"; 
+  }else{ 
+    receivedData.DebugM = "no b"; 
+  } 
+  // ========== send info ========= 
+  receivedData.Rtx = Sent_tx; 
+  receivedData.Rty = Sent_ty; 
+  receivedData.Rtz = Sent_tz;   
+  receivedData.Rrx = Sent_rx;   
+  receivedData.Rry = Sent_ry;  
+  receivedData.Rrz = Sent_rz; 
+  receivedData.Rdist = Sent_dist; 
+  send_var_once(); 
+  print_received_Data(); 
+   
+  //------------------------------------------------------------------------------------- 
+  control_motion(); 
+  //------------------------------------------------------------------------------------- 
+} 
+// ================================== ^ Main loop ^ =================================================== 
+void control_motion(){ 
+  // vertical motor 
+  motorVertical_L->setSpeed(abs(Rec_pwm1)); 
+  motorVertical_R->setSpeed(abs(Rec_pwm2));   
+   
+  if (Rec_dir1 == "+"){ 
+    motorVertical_L->run(BACKWARD); // up  
+  }else if (Rec_dir1 == "-"){ 
+    motorVertical_L->run(FORWARD); // down  
+  } 
+ 
+  if (Rec_dir2 == "+"){  
+    motorVertical_R->run(FORWARD); 
+  }else if (Rec_dir2 == "-"){ 
+    motorVertical_R->run(BACKWARD);     
+  } 
+ 
+  // horizontal motor 
+  motorLeft->setSpeed(abs(Rec_pwm3)); 
+  motorRight->setSpeed(abs(Rec_pwm4)); 
+ 
+  if (Rec_dir3 == "+"){  
+    motorLeft->run(BACKWARD); // make it move forward  
+  }else if (Rec_dir3 == "-"){ 
+    motorLeft->run(FORWARD);   // make it move backward   
+  } 
+ 
+  if (Rec_dir4 == "+"){  
+    motorRight-> run(FORWARD);  // make it move forward  
+  }else if (Rec_dir4 == "-"){ 
+    motorRight-> run(BACKWARD); // make it move backward  
+  } 
+   
+} 
+ 
+void print_received_Data(){ 
+  if (print_count == 0){ 
+  Serial.print("Rec_pwm1:"); 
+  Serial.println(Rec_pwm1); 
+  Serial.print("Rec_pwm2:"); 
+  Serial.println(Rec_pwm2); 
+  Serial.print("Rec_pwm3:"); 
+  Serial.println(Rec_pwm3); 
+  Serial.print("Rec_pwm4:"); 
+  Serial.println(Rec_pwm4);  
+  Serial.print("Rec_dir1:"); 
+  Serial.println(Rec_dir1);  
+  Serial.print("Rec_dir2:"); 
+  Serial.println(Rec_dir2);  
+  Serial.print("Rec_dir3:"); 
+  Serial.println(Rec_dir3); 
+  Serial.print("Rec_dir4:"); 
+  Serial.println(Rec_dir4);     
+Serial.println("_______________________"); 
+  } 
+  print_count +=1; 
+} 
+ 
+void send_var_once(){ 
+  if(count_var==0){ 
+    send_message();  
+    count_var+=1; 
+  } 
+} 
+ 
+void send_message_once(){ 
+  if(count==0){ 
+    send_message();  
+    count+=1; 
+    receivedData.DebugM = ""; 
+  } 
+} 
+ 
+void send_message(){ 
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &receivedData, sizeof(receivedData)); 
+  // esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData)); 
+  //------------------------------------------------------------------------------------- 
+  if (result == ESP_OK) { 
+    Serial.println("Sent with success"); 
+  } 
+  else { 
+    Serial.println("Error sending the data"); 
+  } 
+  //------------------------------------------------------------------------------------- 
+  delay(50); // delay 50 ms after send out the message  
+}