From bbd266305eafa1fd1e2ed1192053b0e495329241 Mon Sep 17 00:00:00 2001
From: zhaoliang <zhz03@g.ucla.edu>
Date: Sat, 16 Oct 2021 11:04:41 -0700
Subject: [PATCH] add print info once function
---
.../ESP32_slave/ESP32_slave.ino | 90 ++++++++++++++++---
1 file changed, 79 insertions(+), 11 deletions(-)
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
index 93104d8..ca27c41 100644
--- 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
@@ -8,7 +8,8 @@ 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;
@@ -55,12 +56,14 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Serial.println(receivedData.ValD);
strData = receivedData.StrD;
valData = receivedData.ValD;
+ count = 0;
+ print_count=0;
}
// =====================================================================================
void setup() {
// Init Serial Monitor
- Serial.begin(115200);
+ Serial.begin(9600);
// Set device as a Wi-Fi Station
WiFi.mode(WIFI_STA);
@@ -94,20 +97,29 @@ void setup() {
void loop()
{
ChangeVariables();
+
if(lidar_thres < 300 && seeking_speed < 50){
- receivedData.DebugM = "catch ball and seeking for goal";
- send_message();
+ receivedData.DebugM = "ca b se g";
+
+ send_message_once();
}else if(lidar_thres < 300 && seeking_speed > 50){
receivedData.DebugM = "catch ball";
- send_message();
+
+ send_message_once();
}else if (lidar_thres > 300 && seeking_speed < 50){
receivedData.DebugM = "seeking";
- send_message();
+ 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));
@@ -147,9 +159,9 @@ 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){
@@ -172,3 +184,59 @@ int getIntVal(String checkData,String Ans,double val,int ori_val){
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 ;
+ }
+}
--
GitLab