add esp now function

Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_modulized/feather_main_test_PID_Lidar_modulized.ino

-
+#include <esp_now.h>
+#include <WiFi.h>
 #include <SparkFun_VL53L1X.h>
 #include <Wire.h>
 #include <Adafruit_MotorShield.h>
@@ -91,11 +92,27 @@ Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
 PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
 PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
 
+//-------------------------------------------------------------------------------------
+typedef struct RxStruct
+{
+  String StrD;
+  double ValD;
+}RxStruct;
+RxStruct receivedData;
+//-------------------------------------------------------------------------------------
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len)
+{
+  memcpy(&receivedData, incomingData, sizeof(receivedData));
+  Serial.print("data:");
+  Serial.println(receivedData.StrD);
+  Serial.println(receivedData.ValD);
+  strData = receivedData.StrD;
+  valData = receivedData.ValD;
+}
+//======================================================================================
 
 void setup() {
   Serial.begin(BAUD_RATE);
-
-  Wire.begin();
   AFMS.begin();  // create with the default frequency 1.6KHz
   interface.begin(); //communication between ESP and OpenMV
   panel.beginPanel();
@@ -115,11 +132,23 @@ void setup() {
   distanceSensor.startRanging();
   distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
   digitalWrite(LED, LOW);
+
+  // ----------------esp now -----------------------
+  Wire.begin();
+  WiFi.mode(WIFI_STA);
+  if (esp_now_init() != ESP_OK)
+  {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
 }
 
 
 // ==============================main loop====================================
-void loop() {  
+void loop() {
+  ChangeVariables();
+  
   panel.singleLED(DEBUG_BASESPEED, base_speed, base_speed, base_speed);
   //if the demo is still ongoing, check to see if there is a desired-color blob
   panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
@@ -185,6 +214,61 @@ void loop() {
 
 
 // ============================== other functions  ====================================
+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);
+  //-------------------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]);
+  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;  
+  //-------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));
+}
+
+double getDoubleVal(String checkData,String Ans,double val,double ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    return val;
+  }else {
+    return ori_val;
+  }
+}
+
+int getIntVal(String checkData,String Ans,double val,int ori_val){
+  if (checkData == Ans){
+    strData = "";
+    valData = 0.0;
+    
+    return (int8_t)val;
+  }else {
+    return ori_val;
+  }
+}
 
 void seeking(){
     // Serial.println("seeking...");