diff --git a/Code/ESP32_Cam/ESP32-CAM_openCV/ESP32-CAM_openCV.ino b/Code/ESP32_Cam/ESP32-CAM_openCV/ESP32-CAM_openCV.ino
index 9aa9d22f4f29077f72e719c9ff5fa0597a507c11..b11150f8d0b4076597d5b6019b55cdf5c0ff4608 100644
--- a/Code/ESP32_Cam/ESP32-CAM_openCV/ESP32-CAM_openCV.ino
+++ b/Code/ESP32_Cam/ESP32-CAM_openCV/ESP32-CAM_openCV.ino
@@ -6,7 +6,11 @@
  
 const char* WIFI_SSID = "lemur";
 const char* WIFI_PASS = "lemur9473";
- 
+
+const char* ssid = "LEMUR_ESP32_CAM_AP_1";
+const char* password = "123456789";
+
+
 WebServer server(80);
  
  
@@ -53,7 +57,9 @@ void handleJpgMid()
   }
   serveJpg();
 }
- 
+
+
+
  
 void  setup(){
   Serial.begin(115200);
@@ -69,17 +75,26 @@ void  setup(){
     bool ok = Camera.begin(cfg);
     Serial.println(ok ? "CAMERA OK" : "CAMERA FAIL");
   }
+  
   WiFi.persistent(false);
   WiFi.mode(WIFI_STA);
-  WiFi.begin(WIFI_SSID, WIFI_PASS);
-  while (WiFi.status() != WL_CONNECTED) {
-    delay(500);
-  }
+  
+//  WiFi.begin(WIFI_SSID, WIFI_PASS);
+//  while (WiFi.status() != WL_CONNECTED) {
+//    delay(500);
+//  }
+
+  WiFi.softAP(ssid, password);
+  
+  Serial.print("http://");
+  Serial.print(WiFi.softAPIP());
+  Serial.println("/cam-lo.jpg");
+  Serial.print("http://");
+  Serial.print(WiFi.softAPIP());
+  Serial.println("/cam-hi.jpg");
   Serial.print("http://");
-  Serial.println(WiFi.localIP());
-  Serial.println("  /cam-lo.jpg");
-  Serial.println("  /cam-hi.jpg");
-  Serial.println("  /cam-mid.jpg");
+  Serial.print(WiFi.softAPIP());
+  Serial.println("/cam-mid.jpg");
  
   server.on("/cam-lo.jpg", handleJpgLo);
   server.on("/cam-hi.jpg", handleJpgHi);
@@ -87,7 +102,8 @@ void  setup(){
  
   server.begin();
 }
- 
+
+
 void loop()
 {
   server.handleClient();
diff --git a/Code/ESP32_communication/Two-way/ESP32_master/ESP32_master.ino b/Code/ESP32_communication/Two-way/ESP32_master/ESP32_master.ino
new file mode 100644
index 0000000000000000000000000000000000000000..43a4944ba8d255f6952e31aabcdb7009a3882680
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/ESP32_master/ESP32_master.ino
@@ -0,0 +1,143 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// REPLACE WITH THE MAC Address of your receiver 
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74};
+String success;
+// Define variables to store BME280 readings to be sent
+String strdata = "";
+String valdata = "";
+String quedata = "";
+int count = 0;
+
+// Define variables to store incoming readings
+String incomingStr = "";
+double incomingVal = 0;
+String incomingDebug = "";
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+  String StrD;
+  double ValD;
+  String DebugM;
+  String QueM;
+  String VarM;
+} struct_message;
+
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message sentData;
+// 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(&sentData, incomingData, sizeof(sentData));
+  // Serial.print("Bytes received: ");
+  // Serial.println(len);
+  // incomingDebug = incomingReadings.DebugM;
+  Serial.print("Var message:");
+  Serial.println(sentData.VarM);
+  Serial.print("Debug message:");
+  Serial.println(sentData.DebugM);
+}
+// =====================================================================================
+
+void setup() {
+  // Init Serial Monitor
+  Serial.begin(9600);
+ 
+  // 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);
+}
+
+// =====================================================================================
+void loop()
+{ 
+  while (Serial.available()>0){
+   int inChar = Serial.read();
+   strdata += char(inChar);
+   delay(10);
+   count +=1;
+   if (count == 3){ 
+    sentData.StrD = strdata;
+    Serial.println(sentData.StrD);
+   }
+  if (isDigit(inChar) || inChar == '.' || inChar == '-'){
+    valdata += char(inChar);
+   }
+  if (inChar == '?'){
+    quedata += char(inChar);
+    sentData.QueM = quedata;
+  }
+  if (inChar == '\n'){ //after message is sent
+    Serial.print("quedata:");
+    Serial.println(quedata);
+
+    if (quedata!= "?"){
+   sentData.ValD = valdata.toDouble();
+   Serial.print("ValD");
+   Serial.println(sentData.ValD);
+   valdata = "";
+   sentData.QueM = "";
+    }else{
+      // sentData.ValD = 0;
+         Serial.print("ValD");
+   Serial.println(sentData.ValD);
+      Serial.print("QueM:");
+      Serial.println(sentData.QueM);
+    }
+
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &sentData, sizeof(sentData));
+  // 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(500);
+  strdata = "";
+  quedata = "";
+  count = 0;
+  }
+  
+  }
+
+  //-------------------------------------------------------------------------------------
+  
+}
diff --git a/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
new file mode 100644
index 0000000000000000000000000000000000000000..2c99ecadfd1a112e1b9f95674165548e82f755d7
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
@@ -0,0 +1,309 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// REPLACE WITH THE MAC Address of your receiver 
+// uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74};
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98};
+String success;
+// Define variables to store BME280 readings to be sent
+String sentDebugM = "";
+int count=0;
+int count_var = 0;
+int print_count = 0;
+int change_count =0;
+// Define variables to store incoming readings
+String strData = "";
+double valData = 0.0;
+String queData = "";
+double Kpx=2, Kix=0.1, Kdx=0.25;
+double Kpy=1, Kiy=0.1, Kdy=0.25;
+int g1 = 0,g2=1,g3=2;
+int goal_id[3] = {g1, g2, g3};
+int8_t Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
+int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
+int base_speed = 70;
+int seeking_speed = 70;
+int lidar_thres = 300; // mm 
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+  String StrD;
+  double ValD;
+  String DebugM;
+  String QueM;
+  String VarM;
+} struct_message;
+
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message receivedData;
+
+// 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));
+  Serial.print("data:");
+  Serial.println(receivedData.StrD);
+  Serial.println(receivedData.ValD);
+  Serial.println(receivedData.QueM);
+  if (receivedData.QueM != "?"){
+  valData = receivedData.ValD;
+  }
+  strData = receivedData.StrD;
+  queData = receivedData.QueM;
+  count = 0;
+  count_var = 0;
+  print_count=0;
+
+  change_count = 0;
+  Serial.print("queData:");
+  Serial.println(queData);
+}
+// =====================================================================================
+
+void setup() {
+  // Init Serial Monitor
+  Serial.begin(9600);
+ 
+  // 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);
+}
+
+// =====================================================================================
+void loop()
+{ if (change_count==0){
+  if (queData != "?"){
+ ChangeVariables();
+ }else if(queData == "?"){
+ QueryVariables();
+ }
+ change_count +=1;
+}
+ 
+ if(lidar_thres < 300 && seeking_speed < 50){
+  receivedData.DebugM = "ca b se g";
+  
+  send_message_once();
+ }else if(lidar_thres < 300 && seeking_speed > 50){
+  receivedData.DebugM = "catch ball";
+  
+  send_message_once();
+ }else if (lidar_thres > 300 && seeking_speed < 50){
+  receivedData.DebugM = "seeking";
+  send_message_once();
+ }
+    
+    print_allvariables();
+  
+  //-------------------------------------------------------------------------------------
+}
+void QueryVariables(){
+  
+  if (strData == "kpx"){
+    receivedData.VarM = String(Kpx);
+  }else if(strData == "kix"){
+    receivedData.VarM = String(Kix);
+  }else if(strData == "kdx"){
+    receivedData.VarM = String(Kdx);
+  }else if(strData == "kpy"){
+    receivedData.VarM = String(Kpy);
+  }else if(strData == "kiy"){
+    receivedData.VarM = String(Kiy);
+  }else if(strData == "kdy"){
+    receivedData.VarM = String(Kdy);
+  }else if(strData == "tha"){
+    receivedData.VarM = String(Lmin);
+  }else if(strData == "thb"){
+    receivedData.VarM = String(Lmax);
+  }else if(strData == "thc"){
+    receivedData.VarM = String(Amin);
+  }else if(strData == "thd"){
+    receivedData.VarM = String(Amax);
+  }else if(strData == "the"){
+    receivedData.VarM = String(Bmin);
+  }else if(strData == "thf"){
+    receivedData.VarM = String(Bmin);
+  }else if(strData == "bsp"){
+    receivedData.VarM = String(base_speed);
+  }else if(strData == "ssp"){
+    receivedData.VarM = String(seeking_speed);
+  }else if(strData == "lth"){
+    receivedData.VarM = String(lidar_thres);
+  }
+
+  send_message_var_once();
+  // queData = "";
+  receivedData.VarM = "";
+}
+
+void send_message_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(500);
+}
+
+void ChangeVariables(){ 
+  // all the variables in this function can be remotely changed
+    //-------------------PID-----------------
+  Kpx = getDoubleVal(strData,"kpx",valData,Kpx);
+  Kix = getDoubleVal(strData,"kix",valData,Kix);
+  Kdx = getDoubleVal(strData,"kdx",valData,Kdx);
+  Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
+  Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
+  Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
+  //-------------------Goal id-----------------
+  g1 = getIntVal(strData,"gda",valData,goal_id[0]);
+  g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
+  g3 = getIntVal(strData,"gdc",valData,goal_id[2]);
+  goal_id[0] = g1;
+  goal_id[1] = g2;
+  goal_id[2] = g3;
+  //-------------------Color threshold-----------------
+  Lmin = getIntVal(strData,"tha",valData,threshold[0]);
+  Lmax = getIntVal(strData,"thb",valData,threshold[1]);
+  Amin = getIntVal(strData,"thc",valData,threshold[2]);
+  Amax = getIntVal(strData,"thd",valData,threshold[3]);
+  Bmin = getIntVal(strData,"the",valData,threshold[4]);
+  Bmax = getIntVal(strData,"thf",valData,threshold[5]);
+  threshold[0] = Lmin;
+  threshold[1] = Lmax;
+  threshold[2] = Amin;
+  threshold[3] = Amax;
+  threshold[4] = Bmin;
+  threshold[5] = Bmax;  
+  //-------base_speed,seeking_speed,lidar_thres-----------------
+  base_speed = abs(getDoubleVal(strData,"bsp",valData,base_speed));
+  seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
+  lidar_thres = getDoubleVal(strData,"lth",valData,lidar_thres);
+}
+
+double getDoubleVal(String checkData,String Ans,double val,double ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    return val;
+  }else {
+    return ori_val;
+  }
+}
+
+int getIntVal(String checkData,String Ans,double val,int ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    
+    return (int8_t)val;
+  }else {
+    return ori_val;
+  }
+}
+
+void print_allvariables(){
+  if (print_count<=1){
+    Serial.println("---------------");
+    Serial.print("base speed:");
+    Serial.print(base_speed);
+    Serial.print("|");
+    Serial.print("seeking speed:");
+    Serial.print(seeking_speed);
+    Serial.print("|");
+    Serial.print("lidar thres:");
+    Serial.println(lidar_thres);
+   
+    
+    Serial.print("threshold:");
+    Serial.print(threshold[0]);
+    Serial.print("|");
+    Serial.print(threshold[1]);
+    Serial.print("|");
+    Serial.print(threshold[2]);
+    Serial.print("|");
+    Serial.print(threshold[3]);
+    Serial.print("|");
+    Serial.print(threshold[4]); 
+    Serial.print("|");   
+    Serial.println(threshold[5]);  
+    
+
+   
+    Serial.print("gid:");
+    Serial.print(goal_id[0]);
+    Serial.print(goal_id[1]);
+    Serial.println(goal_id[2]);
+  
+    
+    Serial.print("Kdx:");
+    Serial.print(Kdx);
+    Serial.print("|"); 
+    Serial.print("Kix:");
+    Serial.print(Kix);
+     Serial.print("|");
+    Serial.print("Kpx:");
+    Serial.print(Kpx);
+     Serial.print("|");
+    Serial.print("Kdy:");
+    Serial.print(Kdy);
+     Serial.print("|");
+    Serial.print("Kiy:");
+    Serial.print(Kiy);
+     Serial.print("|");
+    Serial.print("Kpy:");
+    Serial.println(Kpy);
+Serial.println("---------------------------");
+    print_count +=1 ;
+  }
+}
diff --git a/Code/ESP32_communication/Two-way/Previous_version/ESP32_master/ESP32_master.ino b/Code/ESP32_communication/Two-way/Previous_version/ESP32_master/ESP32_master.ino
new file mode 100644
index 0000000000000000000000000000000000000000..98d9838723967111fa292d8325cef980f09478fb
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/Previous_version/ESP32_master/ESP32_master.ino
@@ -0,0 +1,121 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// REPLACE WITH THE MAC Address of your receiver 
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74};
+String success;
+// Define variables to store BME280 readings to be sent
+String strdata = "";
+String valdata = "";
+int count = 0;
+
+// Define variables to store incoming readings
+String incomingStr = "";
+double incomingVal = 0;
+String incomingDebug = "";
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+  String StrD;
+  double ValD;
+  String DebugM;
+} struct_message;
+
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message sentData;
+// 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(&sentData, incomingData, sizeof(sentData));
+  // Serial.print("Bytes received: ");
+  // Serial.println(len);
+  // incomingDebug = incomingReadings.DebugM;
+  Serial.print("Debug message:");
+  Serial.println(sentData.DebugM);
+}
+// =====================================================================================
+
+void setup() {
+  // Init Serial Monitor
+  Serial.begin(9600);
+ 
+  // 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);
+}
+
+// =====================================================================================
+void loop()
+{ 
+  while (Serial.available()>0){
+   int inChar = Serial.read();
+   strdata += char(inChar);
+   delay(10);
+   count +=1;
+   if (count == 3){ 
+    sentData.StrD = strdata;
+    Serial.println(sentData.StrD);
+   }
+  if (isDigit(inChar) || inChar == '.' || inChar == '-'){
+    valdata += char(inChar);
+   }
+  
+  if (inChar == '\n'){ //after message is sent
+   sentData.ValD = valdata.toDouble();
+   Serial.println(sentData.ValD);
+   
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &sentData, sizeof(sentData));
+  // 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(500);
+  strdata = "";
+  valdata = "";
+  count = 0;
+  }
+  
+  }
+
+  //-------------------------------------------------------------------------------------
+  
+}
diff --git a/Code/ESP32_communication/Two-way/Previous_version/ESP32_slave/ESP32_slave.ino b/Code/ESP32_communication/Two-way/Previous_version/ESP32_slave/ESP32_slave.ino
new file mode 100644
index 0000000000000000000000000000000000000000..ca27c411a35aa19806e9a67eb610c00315228eb0
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/Previous_version/ESP32_slave/ESP32_slave.ino
@@ -0,0 +1,242 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// REPLACE WITH THE MAC Address of your receiver 
+// uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74};
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98};
+String success;
+// Define variables to store BME280 readings to be sent
+String sentDebugM = "";
+int count=0;
+int print_count = 0;
+// Define variables to store incoming readings
+String strData = "";
+double valData = 0.0;
+double Kpx=2, Kix=0.1, Kdx=0.25;
+double Kpy=1, Kiy=0.1, Kdy=0.25;
+int g1 = 0,g2=1,g3=2;
+int goal_id[3] = {g1, g2, g3};
+int8_t Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
+int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
+int base_speed = 70;
+int seeking_speed = 70;
+int lidar_thres = 300; // mm 
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+  String StrD;
+  double ValD;
+  String DebugM;
+} struct_message;
+
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message receivedData;
+
+// 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));
+  Serial.print("data:");
+  Serial.println(receivedData.StrD);
+  Serial.println(receivedData.ValD);
+  strData = receivedData.StrD;
+  valData = receivedData.ValD;
+  count = 0;
+  print_count=0;
+}
+// =====================================================================================
+
+void setup() {
+  // Init Serial Monitor
+  Serial.begin(9600);
+ 
+  // 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);
+}
+
+// =====================================================================================
+void loop()
+{ 
+ ChangeVariables();
+ 
+ if(lidar_thres < 300 && seeking_speed < 50){
+  receivedData.DebugM = "ca b se g";
+  
+  send_message_once();
+ }else if(lidar_thres < 300 && seeking_speed > 50){
+  receivedData.DebugM = "catch ball";
+  
+  send_message_once();
+ }else if (lidar_thres > 300 && seeking_speed < 50){
+  receivedData.DebugM = "seeking";
+  send_message_once();
+ }
+    
+    print_allvariables();
+  
+  //-------------------------------------------------------------------------------------
+}
+void send_message_once(){
+    if(count==0){
+  send_message(); count+=1;
+  }
+}
+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(500);
+}
+void ChangeVariables(){ 
+  // all the variables in this function can be remotely changed
+    //-------------------PID-----------------
+  Kpx = getDoubleVal(strData,"kpx",valData,Kpx);
+  Kix = getDoubleVal(strData,"kix",valData,Kix);
+  Kdx = getDoubleVal(strData,"kdx",valData,Kdx);
+  Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
+  Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
+  Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
+  //-------------------Goal id-----------------
+  g1 = getIntVal(strData,"gda",valData,goal_id[0]);
+  g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
+  g3 = getIntVal(strData,"gdc",valData,goal_id[2]);
+  goal_id[0] = g1;
+  goal_id[1] = g2;
+  goal_id[2] = g3;
+  //-------------------Color threshold-----------------
+  Lmin = getIntVal(strData,"tha",valData,threshold[0]);
+  Lmax = getIntVal(strData,"thb",valData,threshold[1]);
+  Amin = getIntVal(strData,"thc",valData,threshold[2]);
+  Amax = getIntVal(strData,"thd",valData,threshold[3]);
+  Bmin = getIntVal(strData,"the",valData,threshold[4]);
+  Bmax = getIntVal(strData,"thf",valData,threshold[5]);
+  threshold[0] = Lmin;
+  threshold[1] = Lmax;
+  threshold[2] = Amin;
+  threshold[3] = Amax;
+  threshold[4] = Bmin;
+  threshold[5] = Bmax;  
+  //-------base_speed,seeking_speed,lidar_thres-----------------
+  base_speed = abs(getDoubleVal(strData,"bsp",valData,base_speed));
+  seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
+  lidar_thres = getDoubleVal(strData,"lth",valData,lidar_thres);
+}
+
+double getDoubleVal(String checkData,String Ans,double val,double ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    return val;
+  }else {
+    return ori_val;
+  }
+}
+
+int getIntVal(String checkData,String Ans,double val,int ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    
+    return (int8_t)val;
+  }else {
+    return ori_val;
+  }
+}
+
+void print_allvariables(){
+  if (print_count<=1){
+    Serial.print("---------------");
+    Serial.print("base speed:");
+    Serial.print(base_speed);
+    Serial.print("|");
+    Serial.print("seeking speed:");
+    Serial.print(seeking_speed);
+    Serial.print("|");
+    Serial.print("lidar thres:");
+    Serial.println(lidar_thres);
+   
+    
+    Serial.print("threshold:");
+    Serial.print(threshold[0]);
+    Serial.print("|");
+    Serial.print(threshold[1]);
+    Serial.print("|");
+    Serial.print(threshold[2]);
+    Serial.print("|");
+    Serial.print(threshold[3]);
+    Serial.print("|");
+    Serial.print(threshold[4]); 
+    Serial.print("|");   
+    Serial.println(threshold[5]);  
+    
+
+   
+    Serial.print("gid:");
+    Serial.print(goal_id[0]);
+    Serial.print(goal_id[1]);
+    Serial.println(goal_id[2]);
+  
+    
+    Serial.print("Kpx:");
+    Serial.print(Kpx);
+    Serial.print("|"); 
+    Serial.print("Kix:");
+    Serial.print(Kix);
+     Serial.print("|");
+    Serial.print("Kdx:");
+    Serial.print(Kdx);
+     Serial.print("|");
+    Serial.print("Kdy:");
+    Serial.print(Kdy);
+     Serial.print("|");
+    Serial.print("Kiy:");
+    Serial.print(Kiy);
+     Serial.print("|");
+    Serial.print("Kpy:");
+    Serial.println(Kpy);
+
+    print_count +=1 ;
+  }
+}
diff --git a/Code/ESP32_communication/Two-way/Test/Test.ino b/Code/ESP32_communication/Two-way/Test/Test.ino
new file mode 100644
index 0000000000000000000000000000000000000000..23a109d26c3334dae97c6e84c276fc79dcf851b4
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/Test/Test.ino
@@ -0,0 +1,102 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// REPLACE WITH THE MAC Address of your receiver 
+uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+// Define variables to store BME280 readings to be sent
+float temperature;
+float humidity;
+float pressure;
+
+// Define variables to store incoming readings
+float incomingTemp;
+float incomingHum;
+float incomingPres;
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+    float temp;
+    float hum;
+    float pres;
+} struct_message;
+
+// Create a struct_message to hold incoming sensor readings
+struct_message incomingReadings;
+
+// 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(&incomingReadings, incomingData, sizeof(incomingReadings));
+  Serial.print("Bytes received: ");
+  Serial.println(len);
+  incomingTemp = incomingReadings.temp;
+  incomingHum = incomingReadings.hum;
+  incomingPres = incomingReadings.pres;
+}
+// =====================================================================================
+
+void setup() {
+  // Init Serial Monitor
+  Serial.begin(115200);
+ 
+  // 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);
+}
+ 
+void loop() {
+ 
+  // Set values to send
+  BME280Readings.temp = temperature;
+  BME280Readings.hum = humidity;
+  BME280Readings.pres = pressure;
+
+  // Send message via ESP-NOW
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &BME280Readings, sizeof(BME280Readings));
+   
+  if (result == ESP_OK) {
+    Serial.println("Sent with success");
+  }
+  else {
+    Serial.println("Error sending the data");
+  }
+  delay(10000);
+}