From 1dd67a8f12564ca3f39672fa1f02d20d1f51d83a Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Sat, 23 Oct 2021 01:30:42 -0700
Subject: [PATCH] the current working slave code
---
.../Previous_low_level/ESP32_slave/Camera.cpp | 91 +++++++++++++++++++
.../Previous_low_level/ESP32_slave/Camera.h | 19 ++++
.../ESP32_slave/ESP32_slave.ino | 13 +--
.../ESP32_slave/utilities.cpp | 83 +++++++++++++++++
.../ESP32_slave/utilities.h | 8 ++
5 files changed, 208 insertions(+), 6 deletions(-)
create mode 100644 Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp
create mode 100644 Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h
create mode 100644 Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp
create mode 100644 Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp
new file mode 100644
index 0000000..fb27cf8
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp
@@ -0,0 +1,91 @@
+#include "Camera.h"
+#include <Wire.h>
+#include <Arduino.h>
+#include <openmvrpc.h>
+
+Camera::Camera(openmv::rpc_i2c_master *intface){
+ interface = intface;
+}
+
+void Camera::exe_face_detection(){
+ struct { uint16_t x, y, w, h; } face_detection_result;
+ if (interface->call_no_args(F("face_detection"), &face_detection_result, sizeof(face_detection_result))) {
+ Serial.print(F("Largest Face Detected [x="));
+ Serial.print(face_detection_result.x);
+ Serial.print(F(", y="));
+ Serial.print(face_detection_result.y);
+ Serial.print(F(", w="));
+ Serial.print(face_detection_result.w);
+ Serial.print(F(", h="));
+ Serial.print(face_detection_result.h);
+ Serial.println(F("]"));
+ }
+}
+
+bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
+ struct { uint16_t cx, cy, rot; } result;
+ if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
+ }
+ if (result.cx == 0 && result.cy == 0 && result.rot == 0){
+ return false;
+ } else {
+ *x_temp = result.cx;
+ *y_temp = result.cy;
+ *angle_temp = result.rot;
+ return true;
+ }
+}
+
+bool Camera::exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max){
+ int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
+ short buff[128 + 1] = {};
+ if (interface->call(F("color_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
+ int i = 0;
+ while (buff[i] != '\0' && i<100) {
+ Serial.print(buff[i]);
+ i++;
+ }
+ Serial.println("");
+ }
+ if (buff[0] == 0){ //no blob detected
+ return false;
+ } else {
+ return true;
+ }
+}
+
+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;
+ }
+}
+
+
+bool Camera::exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz){
+ int8_t goalid[3] = {goal1, goal2, goal3};
+ struct { uint16_t cid, ctx, cty, ctz, crx, cry, crz; } goalfinder_result;
+ if(interface->call(F("goalfinder"), goalid, sizeof(goalid), &goalfinder_result, sizeof(goalfinder_result))){
+
+ }
+ if (goalfinder_result.crx == 0 && goalfinder_result.cry == 0){
+ return false;
+ } else {
+ id = goalfinder_result.cid;
+ tx = goalfinder_result.ctx;
+ ty = goalfinder_result.cty;
+ tz = goalfinder_result.ctz;
+ rx = goalfinder_result.crx;
+ ry = goalfinder_result.cry;
+ rz = goalfinder_result.crz;
+ return true;
+ }
+}
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h
new file mode 100644
index 0000000..5a4536e
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h
@@ -0,0 +1,19 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+#include <openmvrpc.h>
+
+class Camera
+{
+ private:
+ openmv::rpc_i2c_master *interface;
+
+ public:
+ Camera(openmv::rpc_i2c_master *intface);
+ void exe_face_detection(); // Face should be about 2ft away.
+ bool exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp);
+ bool exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max);
+ bool 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);
+ bool exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz); //optional to add tag size as parameter?
+
+};
+#endif
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
index 9f133bb..37e23c4 100644
--- a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
@@ -11,7 +11,8 @@
// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xCF, 0x00};
-uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+ uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+// uint8_t broadcastAddress[] = {0xc4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
String success;
// Define variables to store incoming readings
@@ -82,10 +83,10 @@ 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 :)";
+ success = "Delivery Success 🙂";
}
else{
- success = "Delivery Fail :(";
+ success = "Delivery Fail ðŸ™";
}
}
@@ -210,7 +211,7 @@ void loop()
Sent_dist = 0;
}
// ========== lidar state info =========
- if (Sent_dist < 300 && Sent_dist < 0){
+ if (Sent_dist < 300 && Sent_dist > 0){
receivedData.DebugM = "found b";
}else{
receivedData.DebugM = "no b";
@@ -284,7 +285,7 @@ void print_received_Data(){
Serial.println(Rec_dir3);
Serial.print("Rec_dir4:");
Serial.println(Rec_dir4);
-Serial.println("_________________________");
+Serial.println("_______________________");
}
print_count +=1;
}
@@ -316,4 +317,4 @@ void send_message(){
}
//-------------------------------------------------------------------------------------
delay(50); // delay 50 ms after send out the message
-}
+}
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp
new file mode 100644
index 0000000..7a90e33
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp
@@ -0,0 +1,83 @@
+#include <math.h>
+
+//Source: http://www.easyrgb.com/index.php?X=MATH&H=01#text1
+void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B )
+{
+ float var_Y = ( l_s + 16. ) / 116.;
+ float var_X = a_s / 500. + var_Y;
+ float var_Z = var_Y - b_s / 200.;
+
+ if ( pow(var_Y,3) > 0.008856 ) var_Y = pow(var_Y,3);
+ else var_Y = ( var_Y - 16. / 116. ) / 7.787;
+ if ( pow(var_X,3) > 0.008856 ) var_X = pow(var_X,3);
+ else var_X = ( var_X - 16. / 116. ) / 7.787;
+ if ( pow(var_Z,3) > 0.008856 ) var_Z = pow(var_Z,3);
+ else var_Z = ( var_Z - 16. / 116. ) / 7.787;
+
+ float X = 95.047 * var_X ; //ref_X = 95.047 Observer= 2°, Illuminant= D65
+ float Y = 100.000 * var_Y ; //ref_Y = 100.000
+ float Z = 108.883 * var_Z ; //ref_Z = 108.883
+
+
+ var_X = X / 100. ; //X from 0 to 95.047 (Observer = 2°, Illuminant = D65)
+ var_Y = Y / 100. ; //Y from 0 to 100.000
+ var_Z = Z / 100. ; //Z from 0 to 108.883
+
+ float var_R = var_X * 3.2406 + var_Y * -1.5372 + var_Z * -0.4986;
+ float var_G = var_X * -0.9689 + var_Y * 1.8758 + var_Z * 0.0415;
+ float var_B = var_X * 0.0557 + var_Y * -0.2040 + var_Z * 1.0570;
+
+ if ( var_R > 0.0031308 ) var_R = 1.055 * pow(var_R , ( 1 / 2.4 )) - 0.055;
+ else var_R = 12.92 * var_R;
+ if ( var_G > 0.0031308 ) var_G = 1.055 * pow(var_G , ( 1 / 2.4 ) ) - 0.055;
+ else var_G = 12.92 * var_G;
+ if ( var_B > 0.0031308 ) var_B = 1.055 * pow( var_B , ( 1 / 2.4 ) ) - 0.055;
+ else var_B = 12.92 * var_B;
+
+ R = var_R * 255.;
+ G = var_G * 255.;
+ B = var_B * 255.;
+
+}
+
+
+
+
+
+//old codes
+
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// Serial.println(SEEKING_SPEED);
+// motorLeft->setSpeed(SEEKING_SPEED);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(SEEKING_SPEED);
+
+// if (abs(Outputx) < 125){
+// motorRight->setSpeed(BASE_SPEED - Outputx);
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(BASE_SPEED + Outputx); //this need to be higher
+// motorLeft->run(BACKWARD);
+// } else if (Outputx >= 125) { //propeller push in opposite direction, moving to the right
+// motorRight->setSpeed(Outputx);
+// motorRight->run(FORWARD);
+// motorLeft->setSpeed(255); //this need to be higher
+// motorLeft->run(BACKWARD);
+// } else {
+// motorRight->setSpeed(255);
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(Outputx); //this need to be higher
+// motorLeft->run(FORWARD);
+// }
+
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// motorLeft->setSpeed(0);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(0);
+// motorRight->run(BACKWARD);
+// motorRight->run(BACKWARD);
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h
new file mode 100644
index 0000000..a1ad9ad
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h
@@ -0,0 +1,8 @@
+#ifndef UTI_H
+#define UTI_H
+
+void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B );
+void translateCodetoThreshold(int code);
+
+
+#endif
--
GitLab