adapt the openmv cam into the slave code

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

@@ -4,6 +4,9 @@
 #include <SparkFun_VL53L1X.h>
 #include <Arduino.h>
 
+#include "Camera.h"
+#include "utilities.h"
+
 // REPLACE WITH THE MAC Address of your receiver (MASTER)
 // Slave: 40:F5:20:44:B6:4C
 // uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xCF, 0x00};
@@ -35,6 +38,9 @@ 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;
@@ -102,11 +108,18 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
   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);
+
 // ==================================== Set up =================================================
 void setup() {
   // Init Serial Monitor
   Serial.begin(115200);
   Wire.begin();
+  interface.begin(); //communication between ESP and OpenMV
   // -------------- LED part --------------------
   pinMode(LED, OUTPUT);
   digitalWrite(LED, HIGH);
@@ -151,12 +164,31 @@ void setup() {
 // ================================== Main loop ===================================================
 void loop()
 { 
-  Sent_tx = 100;
-  Sent_ty = 200;
-  Sent_tz = 40; // cm
-  Sent_rx = 111;
-  Sent_ry = 222;
-  Sent_rz = 333;
+  // ========== 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; 
+  }
+
   Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
 
   if (Sent_dist < 300){