diff --git a/Code/Control/Main_Code/Camera.cpp b/Code/Control/Main_Code/Camera.cpp
index fb27cf808991a0b2083ca36f412ba1aa088527e6..8849608cb61941f1543e71281edd542a2b50dccc 100644
--- a/Code/Control/Main_Code/Camera.cpp
+++ b/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;
}
diff --git a/Code/Control/Main_Code/CameraDefn.h b/Code/Control/Main_Code/CameraDefn.h
index 767cd9e6ea38c20290879497eda3739b6792f295..21809eb1f3c1cbc000b23ddc144d19741808477f 100644
--- a/Code/Control/Main_Code/CameraDefn.h
+++ b/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);
}
diff --git a/Code/Control/Main_Code/Constants.h b/Code/Control/Main_Code/Constants.h
index 232fa65b3d861d1b1f724d12eaa54a003e931e78..c44928f52b327a55e4b2d93428e70b91d879cdf2 100644
--- a/Code/Control/Main_Code/Constants.h
+++ b/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
diff --git a/Code/Control/Main_Code/GroundComm.h b/Code/Control/Main_Code/GroundComm.h
new file mode 100644
index 0000000000000000000000000000000000000000..35443625941982eb39549b81b29fd803cc58539a
--- /dev/null
+++ b/Code/Control/Main_Code/GroundComm.h
@@ -0,0 +1,351 @@
+#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
diff --git a/Code/Control/Main_Code/Main_Code.ino b/Code/Control/Main_Code/Main_Code.ino
index 49f46f1cb05748ea53c0da8756db7bb18b86b1fe..a80d732b752d68af2ff5dfb9fc33227bdaa8c870 100644
--- a/Code/Control/Main_Code/Main_Code.ino
+++ b/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();
}
}
diff --git a/Code/Control/Main_Code/PID.h b/Code/Control/Main_Code/PID.h
index 2aa047880f5adc7ee0e2c75df273288b5f6f23f6..a20469f67d2f6511a55c1835d1d2b6782fab4563 100644
--- a/Code/Control/Main_Code/PID.h
+++ b/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);
diff --git a/Code/Control/Main_Code/Propulsion.h b/Code/Control/Main_Code/Propulsion.h
index e37903f5e9300b0bd6b56263c91f2e81c8f70b15..02a6d3d40c3576fb5e9ab6e4c190f92df77f20bd 100644
--- a/Code/Control/Main_Code/Propulsion.h
+++ b/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