From 9966836fb7ea09ac56c08212be0e00c4b5af4435 Mon Sep 17 00:00:00 2001
From: Zhaoliang <zhz03@g.ucla.edu>
Date: Fri, 15 Oct 2021 14:44:11 -0700
Subject: [PATCH] uploaded the two-way communication

---
 .../Two-way/ESP32_master/ESP32_master.ino     | 139 ++++++++++++++
 .../Two-way/ESP32_slave/ESP32_slave.ino       | 174 ++++++++++++++++++
 .../ESP32_communication/Two-way/Test/Test.ino | 102 ++++++++++
 3 files changed, 415 insertions(+)
 create mode 100644 Code/ESP32_communication/Two-way/ESP32_master/ESP32_master.ino
 create mode 100644 Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
 create mode 100644 Code/ESP32_communication/Two-way/Test/Test.ino

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 0000000..711d662
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/ESP32_master/ESP32_master.ino
@@ -0,0 +1,139 @@
+#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(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()
+{ 
+  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;
+  }
+  
+  }
+
+  //-------------------------------------------------------------------------------------
+  
+}
+
+/*
+void loop() {
+ 
+
+
+  // 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);
+}
+*/
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 0000000..93104d8
--- /dev/null
+++ b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
@@ -0,0 +1,174 @@
+#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 = "";
+
+// 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;
+}
+// =====================================================================================
+
+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()
+{ 
+ ChangeVariables();
+ if(lidar_thres < 300 && seeking_speed < 50){
+  receivedData.DebugM = "catch ball and seeking for goal";
+  send_message();
+ }else if(lidar_thres < 300 && seeking_speed > 50){
+  receivedData.DebugM = "catch ball";
+  send_message();
+ }else if (lidar_thres > 300 && seeking_speed < 50){
+  receivedData.DebugM = "seeking";
+  send_message();
+ }
+ 
+  //-------------------------------------------------------------------------------------
+}
+
+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(getIntVal(strData,"bsp",valData,base_speed));
+  seeking_speed = abs(getIntVal(strData,"ssp",valData,seeking_speed));
+  lidar_thres = abs(getIntVal(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;
+  }
+}
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 0000000..23a109d
--- /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);
+}
-- 
GitLab