diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_modulized/feather_main_test_PID_Lidar_modulized.ino b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_modulized/feather_main_test_PID_Lidar_modulized.ino index f99485db4f54f081c4a73d8d07c05096451a5224..3aad35fcf46f268f14d770d6d20f9f4685a4141a 100644 --- a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_modulized/feather_main_test_PID_Lidar_modulized.ino +++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_modulized/feather_main_test_PID_Lidar_modulized.ino @@ -1,4 +1,5 @@ - +#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...");