Integrated ESP-Cam code to modularized code

Code/Control/Main_Code/Camera.cpp

@@ -55,18 +55,17 @@ bool Camera::exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_
 }
 
 bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y){
-  int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
-  struct { uint16_t cx, cy; } color_detection_result;
-  if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) {
-    
-  }
-  x = color_detection_result.cx;
-  y = color_detection_result.cy;
-  if (x == 0 && y == 0){
-    return false;
-  } else {
-    return true;
-  }
+	int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
+	struct { uint16_t cx, cy; } color_detection_result;
+	if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) {
+	
+	}
+	x = color_detection_result.cx;
+	y = color_detection_result.cy;
+	if (x == 0 && y == 0){
+		return false;
+	}
+	return true;
 }
 
 

Code/Control/Main_Code/CameraDefn.h

@@ -13,7 +13,7 @@ void CameraDefn_Setup(){
   interface.begin(); //communication between ESP and OpenMV
 }
 
-float CameraDefn_BallDetect(int x, int y){
+bool CameraDefn_BallDetect(int& x, int& y){
   return cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y);
 }
 

Code/Control/Main_Code/Constants.h

@@ -4,6 +4,7 @@
 #include <vector>
 
 // Identify all the global constants that will be used by the robot
+const bool ML_BALL_DETECT = true;
 const int BAUD_RATE = 115200;
 const int MAX_SPEED = 255;
 const int SEEKING_SPEED = 70;
@@ -13,6 +14,7 @@ const int ENDDEMO_TAG = 0;
 const uint32_t THISNODE_ID = 88789821;
 int BASE_SPEED = 50; //125;
 int GROUND_LEVEL = 1000; // Pressure at ground level
+int BALL_DETECT_GAP = 500;
 
 // The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;
@@ -26,6 +28,6 @@ const int DEBUG_VERTICALSPEED = 17;
 const int DEBUG_RSPEED = 16;
 const int DEBUG_LSPEED = 24;
 
-int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
+//int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
 
 #endif

Code/Control/Main_Code/GroundComm.h

+#ifndef GROUNDCOMM_H
+#define GROUNDCOMM_H
+
+#include <string>
+#include <WiFi.h>
+#include <Wire.h>
+#include <Arduino.h>
+#include <esp_now.h>
+
+// REPLACE WITH THE MAC Address of your receiver (MASTER)
+// Slave: 40:F5:20:44:B6:4C
+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 count_var = 0;
+int print_count = 0;
+int change_count = 0;
+
+// Define variables to store incoming readings
+String strData = "";
+double valData = 0.0;
+String queData = "";
+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};
+int8_t Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
+int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
+int base_speed = 70;
+int seeking_speed = 70;
+int lidar_thres = 300; // mm 
+int gbx = 0, gby = 0;
+
+double ballDetectTime = 0.0;
+
+// =====================================================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+  String StrD;
+  double ValD;
+  String DebugM;
+  String QueM;
+  String VarM;
+} struct_message;
+
+// Function Declarations
+void ChangeVariables();
+void QueryVariables();
+void send_message();
+void send_message_var_once();
+double getDoubleVal(String checkData, String Ans, double val, double ori_val);
+int getIntVal(String checkData,String Ans,double val,int ori_val);
+
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message receivedData;
+
+// Callback when data is sent
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+  Serial.print("\r\nLast Packet Send Status:\t");
+  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
+  
+  if (status ==0){
+    success = "Delivery Success :)";
+  }
+  else{
+    success = "Delivery Fail :(";
+  }
+}
+
+
+// Callback when data is received
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
+  memcpy(&receivedData, incomingData, sizeof(receivedData));
+  Serial.print("\ndata:");
+  Serial.println(receivedData.StrD);
+  Serial.println(receivedData.ValD);
+  Serial.println(receivedData.QueM);
+  
+  if (receivedData.QueM != "?"){
+    valData = receivedData.ValD;
+  }
+  
+  strData = receivedData.StrD;
+  queData = receivedData.QueM;
+  
+  count = 0;
+  count_var = 0;
+  print_count=0;
+  change_count = 0;
+  
+  Serial.print("queData:");
+  Serial.println(queData);
+}
+// =====================================================================================
+
+
+void GroundComm_Setup(){
+  // Set device as a Wi-Fi Station
+  WiFi.mode(WIFI_STA);
+  
+  // Init ESP-NOW
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  
+  // Once ESPNow is successfully Init, we will register for Send CB to
+  // get the status of Trasnmitted packet
+  esp_now_register_send_cb(OnDataSent);
+  
+  // Register peers
+  esp_now_peer_info_t peerInfo;
+  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+  peerInfo.channel = 0;  
+  peerInfo.encrypt = false;
+  
+  // Add peer        
+  if (esp_now_add_peer(&peerInfo) != ESP_OK){
+    Serial.println("Failed to add peer");
+    return;
+  }
+  
+  // Register for a callback function that will be called when data is received
+  esp_now_register_recv_cb(OnDataRecv);
+}
+
+void GroundComm_LoopStart(){
+  if (change_count == 0){
+    if (queData != "?"){
+      ChangeVariables();
+    }
+  else if(queData == "?"){
+      QueryVariables();
+    }
+    change_count +=1;
+  }
+}
+
+bool GroundComm_BallDetect(int &x, int& y){
+  Serial.println("In GroundComm_BallDetect");
+  x = gbx;
+  y = gby;
+  Serial.println(x);
+  Serial.println(y);
+  if (x == -1)
+    return false;
+  return true;
+}
+
+void QueryVariables(){
+  if (strData == "kpx"){
+    receivedData.VarM = String(Kpx);
+  }else if(strData == "kix"){
+    receivedData.VarM = String(Kix);
+  }else if(strData == "kdx"){
+    receivedData.VarM = String(Kdx);
+  }else if(strData == "kpy"){
+    receivedData.VarM = String(Kpy);
+  }else if(strData == "kiy"){
+    receivedData.VarM = String(Kiy);
+  }else if(strData == "kdy"){
+    receivedData.VarM = String(Kdy);
+  }else if(strData == "tha"){
+    receivedData.VarM = String(Lmin);
+  }else if(strData == "thb"){
+    receivedData.VarM = String(Lmax);
+  }else if(strData == "thc"){
+    receivedData.VarM = String(Amin);
+  }else if(strData == "thd"){
+    receivedData.VarM = String(Amax);
+  }else if(strData == "the"){
+    receivedData.VarM = String(Bmin);
+  }else if(strData == "thf"){
+    receivedData.VarM = String(Bmin);
+  }else if(strData == "bsp"){
+    receivedData.VarM = String(base_speed);
+  }else if(strData == "ssp"){
+    receivedData.VarM = String(seeking_speed);
+  }else if(strData == "lth"){
+    receivedData.VarM = String(lidar_thres);
+  }
+
+  send_message_var_once();
+  // queData = "";
+  receivedData.VarM = "";
+}
+
+void send_message_var_once(){
+  if(count_var==0){
+    send_message(); 
+    count_var+=1;
+  }
+}
+
+void send_message_once(){
+  if(count==0){
+    send_message(); 
+    count+=1;
+    receivedData.DebugM = "";
+  }
+}
+
+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));
+  //-------------------------------------------------------------------------------------
+  if (result == ESP_OK) {
+    Serial.println("Sent with success");
+  }
+  else {
+    Serial.println("Error sending the data");
+  }
+  //-------------------------------------------------------------------------------------
+  delay(100);
+}
+
+
+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(getDoubleVal(strData,"bsp",valData,base_speed));
+  seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
+  lidar_thres = getDoubleVal(strData,"lth",valData,lidar_thres);
+
+  //------ green ball x, y----
+  gbx = getDoubleVal(strData,"gbx",valData,gbx);
+  gby = getDoubleVal(strData,"gby",valData,gby);
+  if (strData == "gbx" || strData == "gby")
+    ballDetectTime = millis();
+}
+
+
+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 print_allvariables(){
+  if (print_count<=1){
+    Serial.println("---------------");
+    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("Kdx:");
+    Serial.print(Kdx);
+    Serial.print("|"); 
+    Serial.print("Kix:");
+    Serial.print(Kix);
+    Serial.print("|");
+    Serial.print("Kpx:");
+    Serial.print(Kpx);
+    Serial.print("|");
+    Serial.print("Kdy:");
+    Serial.print(Kdy);
+    Serial.print("|");
+    Serial.print("Kiy:");
+    Serial.print(Kiy);
+    Serial.print("|");
+    Serial.print("Kpy:");
+    Serial.println(Kpy);
+    Serial.print("|");
+    
+    Serial.print("gbx:");
+    Serial.print(gbx);
+    Serial.print("|");
+    Serial.print("gby:");
+    Serial.println(gby);
+    
+    Serial.println("---------------------------\n");
+    print_count +=1 ;
+  }
+}
+
+#endif

Code/Control/Main_Code/Main_Code.ino

@@ -8,6 +8,7 @@
 #include "Constants.h"
 #include "utilities.h"
 #include "CameraDefn.h"
+#include "GroundComm.h"
 #include "Propulsion.h"
 #include "ColorDetect.h"
 //#include "Localization.h"
@@ -15,13 +16,14 @@
 using namespace std;
 
 // Identify all the variables that will be used by the robot
-int   findcolor       = 1; //based on the same code from openMV. Determine the color you want to find
-int   pauseState      = 0;
-int   displayTracking = 0;
-int   ballCapture     = 0;
-int   ballDetect      = 0;
-int   goalFind        = 0;
-float altitude        = 0;
+int     findcolor       = 1; //based on the same code from openMV. Determine the color you want to find
+int     pauseState      = 0;
+int     displayTracking = 0;
+int     ballCapture     = 0;
+int     ballDetect      = 0;
+int     goalFind        = 0;
+float   altitude        = 0;
+double *ballProps;
 
 // Function Declarations
 
@@ -30,6 +32,7 @@ void setup() {
   Wire.begin();
   AFMS.begin();  // create with the default frequency 1.6KHz
   
+  GroundComm_Setup();
   CameraDefn_Setup();
   ColorDetect_Setup(findcolor);
   Debug_Setup();
@@ -49,6 +52,7 @@ void loop(){
   #ifdef MESH_H
     Mesh_LoopStart();
   #endif
+  GroundComm_LoopStart();
   
   /* --------- PAUSE STATE --------- */
   while(pauseState == 1){
@@ -68,8 +72,11 @@ void loop(){
   int8_t goal[3] = {0,1,2};
   
   ballCapture = LIDAR_BallCapture();
-  ballDetect  = CameraDefn_BallDetect(x, y);
   goalFind    = CameraDefn_GoalFind(goal, id, tx, ty, tz, rx, ry, rz);
+  if (ML_BALL_DETECT)
+    ballDetect  = GroundComm_BallDetect(x, y);
+  else
+    ballDetect  = CameraDefn_BallDetect(x, y);
   /* ----- END INITIALIZATION ------ */
   
   /* -------- STANDBY STATE -------- */
@@ -90,6 +97,7 @@ void loop(){
   /* ---------- MAIN LOGIC --------- */
   if(ballCapture){ // Ball captured
 	  if(goalFind){ // Ball captured and Goal found
+      Serial.println("  Ball captured and Goal found");
       Debug_GoalFound();
       PID_GoalFound(tx/1.00, ty/1.00);
       #ifdef LOCALIZATION_H
@@ -98,12 +106,16 @@ void loop(){
 	    Propulsion_GoalFound();
     }
     else { // Ball captured but Goal not found
+      Serial.println("  Ball captured but Goal not found");
       Debug_GoalSeeking();
       Propulsion_GoalSeeking();
     }
   }
   else { // Ball not captured
 	  if(ballDetect) { // Ball not captured but detected
+      Serial.println("  Ball not captured but detected");
+      Serial.print(x);
+      Serial.println(y);
       Debug_BallDetected();
       #ifdef LOCALIZATION_H
         Localization_BallDetected();
@@ -112,6 +124,7 @@ void loop(){
       Propulsion_BallDetected();
     }
     else { // Ball not captured and not detected
+      Serial.println("  Ball not captured and not detected");
       Propulsion_BallSeeking();
     }
   }

Code/Control/Main_Code/PID.h

@@ -3,15 +3,16 @@
 
 #include <string>
 #include "Debug.h"
-#include "Constants.h"
 #include <PID_v1.h>
+#include "Constants.h"
+#include "GroundComm.h"
 
 double Setpointx, Inputx, Outputx;
 double Setpointy, Inputy, Outputy;
 
 //Specify the links and initial tuning parameters
-double Kpx=2, Kix=0.05, Kdx=0.25;
-double Kpy=2, Kiy=0.1,  Kdy=0.25;
+//double Kpx=2, Kix=0.05, Kdx=0.25;
+//double Kpy=2, Kiy=0.1,  Kdy=0.25;
 PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
 PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
 

Code/Control/Main_Code/Propulsion.h

@@ -3,7 +3,7 @@
 
 #include "Debug.h"
 #include "Constants.h"
-#include "Localization.h"
+//#include "Localization.h"
 #include <Adafruit_MotorShield.h>
 
 // Function Declarations
@@ -29,8 +29,11 @@ void Propulsion_GoalFound(){
 }
 
 void Propulsion_GoalSeeking(){
-//  move_V(0);
-  hover(altitudeCalc());
+  #ifdef LOCALIZATION_H
+    hover(altitudeCalc());
+  #else
+    move_V(0);
+  #endif
   move_H(SEEKING_SPEED, 0);
 }
 
@@ -40,8 +43,11 @@ void Propulsion_BallDetected(){
 }
 
 void Propulsion_BallSeeking(){
-//  move_V(0);
-  hover(altitudeCalc());
+  #ifdef LOCALIZATION_H
+    hover(altitudeCalc());
+  #else
+    move_V(0);
+  #endif
   move_H(SEEKING_SPEED, 0);
 }
 
@@ -94,9 +100,11 @@ void move_H(int V_Rot, int V_Trn){
   motor_HR->setSpeed(min(MAX_SPEED, abs(lspeed)));
 }
 
+#ifdef LOCALIZATION_H  
 void hover(float altitude){
   float hoverAltitude = altitudeControl();
   move_V(BASE_SPEED*(hoverAltitude-altitudeCalc()));
 }
+#endif
 
 #endif