From 84d6e4bf2e21f9e8a536190e2c69127da76686be Mon Sep 17 00:00:00 2001
From: zhaoliang <zhz03@g.ucla.edu>
Date: Fri, 15 Oct 2021 17:30:37 -0700
Subject: [PATCH] Try to query the variables but failed
---
.../Two-way/ESP32_slave/ESP32_slave.ino | 100 +++++++++++++++++-
1 file changed, 96 insertions(+), 4 deletions(-)
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 2954246..8df80af 100644
--- a/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
+++ b/Code/ESP32_communication/Two-way/ESP32_slave/ESP32_slave.ino
@@ -12,10 +12,11 @@ String sentDebugM = "";
// 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};
+int goal_id[3] = {0,1,2};
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;
@@ -29,6 +30,7 @@ typedef struct struct_message {
String StrD;
double ValD;
String DebugM;
+ String QueM;
} struct_message;
// Create a struct_message to hold incoming sensor readings
@@ -53,8 +55,10 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Serial.print("data:");
Serial.println(receivedData.StrD);
Serial.println(receivedData.ValD);
+ Serial.println(receivedData.QueM);
strData = receivedData.StrD;
valData = receivedData.ValD;
+ queData = receivedData.QueM;
}
// =====================================================================================
@@ -88,12 +92,31 @@ 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()
{
- ChangeVariables();
+ if (queData != "?"){
+ ChangeVariables();}
+ /*
+ if (queData == "?"){
+ QueryVariables();}
+ */
if(lidar_thres < 200 && seeking_speed < 50){
receivedData.DebugM = "ca b se g";
send_message();
@@ -104,7 +127,20 @@ void loop()
receivedData.DebugM = "seeking";
send_message();
}
-
+/*
+ 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);
+ */
//-------------------------------------------------------------------------------------
}
@@ -117,15 +153,67 @@ 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]);
@@ -149,14 +237,18 @@ void ChangeVariables(){
//-------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));
+ 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;
}
--
GitLab