test other variables

Code/ESP32_communication/test_more_variables/ESP32_slave/ESP32_slave.ino

@@ -14,7 +14,9 @@ int g1 = 0,g2=1,g3=2;
 int goal_id[3] = {g1, g2, g3};
 int Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
 int threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
-
+int base_speed = 70;
+int seeking_speed = 70;
+int lidar_thres = 300; // mm 
 //-------------------------------------------------------------------------------------
 typedef struct RxStruct
 {
@@ -47,19 +49,21 @@ void setup()
 }
 //======================================================================================
 void loop()
-{ 
+{ //-------------------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]);
@@ -72,7 +76,21 @@ void loop()
   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));
+  
+    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("|");
@@ -85,6 +103,7 @@ void loop()
     Serial.print(threshold[4]); 
     Serial.print("|");   
     Serial.println(threshold[5]);  
+  */
   /*
     Serial.print("gid:");
     Serial.print(goal_id[0]);