test the color threshold

Code/ESP32_communication/test_more_variables/ESP32_slave/ESP32_slave.ino

@@ -12,6 +12,9 @@ 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 Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
+int threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
+
 //-------------------------------------------------------------------------------------
 typedef struct RxStruct
 {
@@ -51,16 +54,43 @@ void loop()
   Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
   Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
   Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
-  g1 = getDoubleVal(strData,"gda",valData,goal_id[0]);
-  g2 = getDoubleVal(strData,"gdb",valData,goal_id[1]);
-  g3 = getDoubleVal(strData,"gdc",valData,goal_id[2]);
+  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;
+  Lmin = getIntVal(strData,"tha",valData,threshold[0]);
+  Lmax = getIntVal(strData,"thb",valData,threshold[1]);
+  Amin = getIntVal(strData,"thc",valData,threshold[2]);
+  Amax = getIntVal(strData,"thd",valData,threshold[3]);
+  Bmin = getIntVal(strData,"the",valData,threshold[4]);
+  Bmax = getIntVal(strData,"thf",valData,threshold[5]);
+  threshold[0] = Lmin;
+  threshold[1] = Lmax;
+  threshold[2] = Amin;
+  threshold[3] = Amax;
+  threshold[4] = Bmin;
+  threshold[5] = Bmax;  
+
+    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.println(Kpx);