diff --git a/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
index 2c99ecadfd1a112e1b9f95674165548e82f755d7..96cb18326de66c27a75b4282bf32759ec0e290c4 100644
--- a/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
+++ b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
@@ -2,23 +2,25 @@
#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};
+// 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};
String success;
+
// Define variables to store BME280 readings to be sent
String sentDebugM = "";
-int count=0;
+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;
+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};
@@ -26,6 +28,9 @@ int base_speed = 70;
int seeking_speed = 70;
int lidar_thres = 300; // mm
+int gbx = 0, gby = 0;
+
+
// =====================================================================================
//Structure the sending data
//Must match the receiver structure
@@ -37,14 +42,17 @@ typedef struct struct_message {
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 :)";
}
@@ -53,31 +61,36 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
}
}
+
// 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.print("\ndata:");
Serial.println(receivedData.StrD);
Serial.println(receivedData.ValD);
Serial.println(receivedData.QueM);
+
if (receivedData.QueM != "?"){
- valData = receivedData.ValD;
+ 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);
+ Serial.begin(115200);
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
@@ -103,40 +116,43 @@ void setup() {
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();
- }
+{
+ if (change_count==0){
+ if (queData != "?"){
+ ChangeVariables();
+ }else if(queData == "?"){
+ QueryVariables();
+ }
- print_allvariables();
+ change_count +=1;
+ }
+
+ // FOR LIDAR
+ 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"){
@@ -174,37 +190,50 @@ void QueryVariables(){
receivedData.VarM = "";
}
+
void send_message_var_once(){
- if(count_var==0){
- send_message(); count_var+=1;
+ if(count_var==0){
+ send_message();
+ count_var+=1;
}
}
+
+
void send_message_once(){
- if(count==0){
- send_message(); count+=1;
- receivedData.DebugM = "";
+ 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");
+ if (result == ESP_OK) {
+ Serial.println("Sent with success");
+ }
+ else {
+ Serial.println("Error sending the data");
+ }
//-------------------------------------------------------------------------------------
- delay(500);
+ delay(100);
}
+
void ChangeVariables(){
// all the variables in this function can be remotely changed
- //-------------------PID-----------------
+
+ //-------------------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]);
@@ -212,6 +241,7 @@ void ChangeVariables(){
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]);
@@ -224,13 +254,20 @@ void ChangeVariables(){
threshold[2] = Amin;
threshold[3] = Amax;
threshold[4] = Bmin;
- threshold[5] = Bmax;
+ 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);
+
+ //------ green ball x, y----
+ gbx = getDoubleVal(strData,"gbx",valData,gbx);
+ gby = getDoubleVal(strData,"gby",valData,gby);
+
}
+
double getDoubleVal(String checkData,String Ans,double val,double ori_val){
if (checkData == Ans){
strData = "";
@@ -241,17 +278,18 @@ double getDoubleVal(String checkData,String Ans,double val,double 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("---------------");
@@ -263,7 +301,6 @@ void print_allvariables(){
Serial.print("|");
Serial.print("lidar thres:");
Serial.println(lidar_thres);
-
Serial.print("threshold:");
Serial.print(threshold[0]);
@@ -278,32 +315,37 @@ void print_allvariables(){
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("|");
Serial.print("Kpx:");
Serial.print(Kpx);
- Serial.print("|");
+ Serial.print("|");
Serial.print("Kdy:");
Serial.print(Kdy);
- Serial.print("|");
+ Serial.print("|");
Serial.print("Kiy:");
Serial.print(Kiy);
- Serial.print("|");
+ Serial.print("|");
Serial.print("Kpy:");
Serial.println(Kpy);
-Serial.println("---------------------------");
+ Serial.print("|");
+
+ Serial.print("gbx:");
+ Serial.print(gbx);
+ Serial.print("|");
+ Serial.print("gby:");
+ Serial.println(gby);
+
+ Serial.println("---------------------------\n");
print_count +=1 ;
}
}