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 8df80af048f8d2ec9d532ec770aa03a8e62e7580..2c99ecadfd1a112e1b9f95674165548e82f755d7 100644
--- a/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
+++ b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
@@ -8,7 +8,10 @@ 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;
@@ -16,7 +19,7 @@ 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] = {0,1,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;
@@ -31,6 +34,7 @@ typedef struct struct_message {
double ValD;
String DebugM;
String QueM;
+ String VarM;
} struct_message;
// Create a struct_message to hold incoming sensor readings
@@ -56,9 +60,18 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Serial.println(receivedData.StrD);
Serial.println(receivedData.ValD);
Serial.println(receivedData.QueM);
- strData = receivedData.StrD;
+ 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);
}
// =====================================================================================
@@ -92,57 +105,86 @@ void setup() {
}
// Register for a callback function that will be called when data is received
esp_now_register_recv_cb(OnDataRecv);
-
- //----------------------------set variable value
- strData = "";
- valData = 0.0;
- queData = "";
- Kpx=2; Kix=0.1; Kdx=0.25;
- Kpy=1; Kiy=0.1; Kdy=0.25;
- g1 = 0,g2=1,g3=2;
-// goal_id[3] = {g1, g2, g3};
- Lmin = 30;Lmax = 100; Amin = -49;Amax = -22;Bmin = 31;Bmax = 127;
- int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
- base_speed = 70;
- seeking_speed = 70;
- lidar_thres = 300; // mm
}
// =====================================================================================
void loop()
-{
+{ if (change_count==0){
if (queData != "?"){
- ChangeVariables();}
- /*
- if (queData == "?"){
- QueryVariables();}
- */
- if(lidar_thres < 200 && seeking_speed < 50){
+ ChangeVariables();
+ }else if(queData == "?"){
+ QueryVariables();
+ }
+ change_count +=1;
+}
+
+ if(lidar_thres < 300 && seeking_speed < 50){
receivedData.DebugM = "ca b se g";
- send_message();
- }else if(lidar_thres < 200 && seeking_speed > 50){
+
+ send_message_once();
+ }else if(lidar_thres < 300 && seeking_speed > 50){
receivedData.DebugM = "catch ball";
- send_message();
- }else if (lidar_thres > 200 && seeking_speed < 50){
+
+ send_message_once();
+ }else if (lidar_thres > 300 && seeking_speed < 50){
receivedData.DebugM = "seeking";
- send_message();
+ send_message_once();
}
-/*
- Serial.print("Kpx:");
- Serial.println(Kpx);
- Serial.print("Kix:");
- Serial.println(Kix);
- Serial.print("Kdx:");
- Serial.println(Kdx);
- Serial.print("Kpy:");
- Serial.println(Kpy);
- Serial.print("Kiy:");
- Serial.println(Kiy);
- Serial.print("Kdy:");
- Serial.println(Kdy);
- */
+
+ 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));
@@ -154,66 +196,15 @@ void send_message(){
delay(500);
}
-void QueryVariables(){
-
- double testVal;
-
- if (strData == "kpx"){
- receivedData.DebugM = String(Kpx);
- }else if(strData = "kix"){
- receivedData.DebugM = String(Kix);
- }else if(strData = "kdx"){
- receivedData.DebugM = String(Kdx);
- }else if(strData = "kpy"){
- receivedData.DebugM = String(Kpy);
- }else if(strData = "kiy"){
- receivedData.DebugM = String(Kiy);
- }else if(strData = "kdy"){
- receivedData.DebugM = String(Kdy);
- }
-
-
- // = String(testVal);
- send_message();
- // strData = "";
- // queData = "";
-}
-
-/*
-double getVaribleVal(String checkData,String Ans,double ori_val){
- if (checkData == Ans){
- return ori_val;
- }else{
- return 999;
- }
-}*/
-
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);
-
- /*
- Serial.print("Kpx:");
- Serial.println(Kpx);
- Serial.print("Kix:");
- Serial.println(Kix);
- Serial.print("Kdx:");
- Serial.println(Kdx);
-
- Serial.print("Kpy:");
- Serial.println(Kpy);
- Serial.print("Kiy:");
- Serial.println(Kiy);
- Serial.print("Kdy:");
- Serial.println(Kdy);
- */
//-------------------Goal id-----------------
g1 = getIntVal(strData,"gda",valData,goal_id[0]);
g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
@@ -235,20 +226,16 @@ void ChangeVariables(){
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));
-
-
+ 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;
}
@@ -264,3 +251,59 @@ int getIntVal(String checkData,String Ans,double val,int ori_val){
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 ;
+ }
+}