diff --git a/Code/BaroLIDAR_Altitude_Code/ESP32_master/ESP32_master.ino b/Code/BaroLIDAR_Altitude_Code/ESP32_master/ESP32_master.ino
new file mode 100644
index 0000000000000000000000000000000000000000..ec3948183eadf8dc8d601975141606d33d49b8e8
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/ESP32_master/ESP32_master.ino
@@ -0,0 +1,207 @@
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+
+// ==================================== broadcast address ============================================
+// MAC Address of the receiver (SLAVE)
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
+// ==================================== global data =================================================
+String success;
+// Define variables to store BME280 readings to be sent
+String strdata = "";
+int count = 0;
+int print_flag = 0;
+
+// ==================================== data structure =================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+ int Rtx;
+ int Rty;
+ int Rtz;
+ int Rrx;
+ int Rry;
+ int Rrz;
+ int Rdist;
+ float RaltBaro;
+ String DebugM;
+ int Spwm1;
+ int Spwm2;
+ int Spwm3;
+ int Spwm4;
+ String Sdir1;
+ String Sdir2;
+ String Sdir3;
+ String Sdir4;
+} struct_message;
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message sentData;
+
+// =================================== send and received function =====================================
+// 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(&sentData, incomingData, sizeof(sentData));
+ // Serial.print("Bytes received: ");
+ // Serial.println(len);
+ Serial.println("SERIAL_IN_START");
+ Serial.println(sentData.Rtx);
+ Serial.println(sentData.Rty);
+ Serial.println(sentData.Rtz);
+ Serial.println(sentData.Rrx);
+ Serial.println(sentData.Rry);
+ Serial.println(sentData.Rrz);
+ Serial.println(sentData.Rdist);
+ Serial.println(sentData.RaltBaro);
+ Serial.println(sentData.DebugM);
+}
+// =================================== setup ==================================================
+
+void setup() {
+ // Init Serial Monitor
+ Serial.begin(115200);
+
+ // 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 peer
+ 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);
+}
+
+// ======================================== main =============================================
+void loop()
+{
+ while (Serial.available()>0){
+ // the data format: 225 225 225 225 + + - -
+ int inChar = Serial.read();
+ strdata += char(inChar);
+ //delay(10);
+ count +=1;
+
+ if (count == 3){
+ sentData.Spwm1 = strdata.toInt();
+ if (print_flag == 1){
+ Serial.println(sentData.Spwm1);
+ }
+ strdata = "";
+ }
+
+ if (count == 6){
+ sentData.Spwm2 = strdata.toInt();
+ if (print_flag == 1){
+ Serial.println(sentData.Spwm2);
+ }
+ strdata = "";
+ }
+
+ if (count == 9){
+ sentData.Spwm3 = strdata.toInt();
+ if (print_flag == 1){
+ Serial.println(sentData.Spwm3);
+ }
+ strdata = "";
+ }
+
+ if (count == 12){
+ sentData.Spwm4 = strdata.toInt();
+ if (print_flag == 1){
+ Serial.println(sentData.Spwm4);
+ }
+ strdata = "";
+ }
+
+ if (count == 13){
+ sentData.Sdir1 = strdata;
+ if (print_flag == 1){
+ Serial.println(sentData.Sdir1);
+ }
+ strdata = "";
+ }
+
+ if (count == 14){
+ sentData.Sdir2 = strdata;
+ if (print_flag == 1){
+ Serial.println(sentData.Sdir2);
+ }
+ strdata = "";
+ }
+
+ if (count == 15){
+ sentData.Sdir3 = strdata;
+ if (print_flag == 1){
+ Serial.println(sentData.Sdir3);
+ }
+ strdata = "";
+ }
+
+ if (count == 16){
+ sentData.Sdir4 = strdata;
+ if (print_flag == 1){
+ Serial.println(sentData.Sdir4);
+ }
+ strdata = "";
+ }
+
+ if (inChar == '\n'){ //after message is sent
+
+ // esp_err_t result =
+ esp_now_send(broadcastAddress, (uint8_t *) &sentData, sizeof(sentData));
+ // 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(50); // delay 50 ms
+ strdata = "";
+ count = 0;
+ }
+
+ }
+
+ //-------------------------------------------------------------------------------------
+
+ // delay(50);
+}
diff --git a/Code/BaroLIDAR_Altitude_Code/ESP32_slave/Camera.cpp b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/Camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56f42d65285a6f3129d8bf37368f094f86682d65
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/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 { int16_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/BaroLIDAR_Altitude_Code/ESP32_slave/Camera.h b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/Camera.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a4536e6d0967e96fd5864e596b8f8f9f0312289
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/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/BaroLIDAR_Altitude_Code/ESP32_slave/ESP32_slave.ino b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/ESP32_slave.ino
new file mode 100644
index 0000000000000000000000000000000000000000..747eb23ac27bc949758579f24bbebb1acd6da068
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/ESP32_slave.ino
@@ -0,0 +1,351 @@
+#include <WiFi.h>
+#include <Wire.h>
+#include <Arduino.h>
+#include <esp_now.h>
+#include <SparkFun_VL53L1X.h>
+#include <Adafruit_MPL3115A2.h>
+
+#include "Camera.h"
+#include "utilities.h"
+#include <Adafruit_MotorShield.h>
+
+// REPLACE WITH THE MAC Address of your receiver (MASTER)
+// Slave: 40:F5:20:44:B6:4C
+// MAC Address of the receiver (MASTER)
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
+String success;
+
+// Define variables to store incoming readings
+String sentDebugM = "";
+int Rec_pwm1;
+int Rec_pwm2;
+int Rec_pwm3;
+int Rec_pwm4;
+String Rec_dir1;
+String Rec_dir2;
+String Rec_dir3;
+String Rec_dir4;
+
+int count = 0;
+int count_var = 0;
+int print_count = 0;
+int change_count =0;
+int Lidar_flag = 0;
+int altBaro_flag = 0;
+
+// Define variables to be sent;
+int Sent_tx = 0;
+int Sent_ty = 0;
+int Sent_tz = 0;
+int Sent_rx = 0;
+int Sent_ry = 0;
+int Sent_rz = 0;
+int Sent_dist = 0;
+float Sent_altBaro = 0;
+
+int8_t g1 = 0,g2=1,g3=2;
+int8_t goal_id[3] = {g1, g2, g3};
+
+// Define Lidar variables
+SFEVL53L1X distanceSensor;
+int budgetIndex = 4;
+int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
+
+// Define Barometer variables
+Adafruit_MPL3115A2 baro;
+
+// Define LED variable
+int LED = LED_BUILTIN;
+
+// ==================================== data structure =================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+ int Rtx;
+ int Rty;
+ int Rtz;
+ int Rrx;
+ int Rry;
+ int Rrz;
+ int Rdist;
+ float RaltBaro;
+ String DebugM;
+ int Spwm1;
+ int Spwm2;
+ int Spwm3;
+ int Spwm4;
+ String Sdir1;
+ String Sdir2;
+ String Sdir3;
+ String Sdir4;
+} struct_message;
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message receivedData;
+
+// =================================== send and received function =====================================
+// 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));
+ // the data format: 225 225 225 225 + + - -
+ // + up / forward
+ // - down / backward
+ Rec_pwm1 = receivedData.Spwm1;
+ Rec_pwm2 = receivedData.Spwm2;
+ Rec_pwm3 = receivedData.Spwm3;
+ Rec_pwm4 = receivedData.Spwm4;
+ Rec_dir1 = receivedData.Sdir1;
+ Rec_dir2 = receivedData.Sdir2;
+ Rec_dir3 = receivedData.Sdir3;
+ Rec_dir4 = receivedData.Sdir4;
+ count_var = 0;
+ count = 0;
+ print_count=0;
+ change_count = 0;
+}
+
+// =============================== All the setup ===========================================
+//Create the interface that will be used by the camera
+openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+Camera cam(&interface);
+// ========================== Motor part ====================================
+// Create the motor shield object with the default I2C address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+// changed
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1); // pwm1
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2); // pwm2
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3); // pwm3
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4); // pwm4
+
+// ==================================== Set up =================================================
+void setup() {
+ // Init Serial Monitor
+ Serial.begin(115200);
+ Wire.begin();
+ interface.begin(); //communication between ESP and OpenMV
+ AFMS.begin(); // create with the default frequency 1.6KHz
+ // -------------- LED part --------------------
+ pinMode(LED, OUTPUT);
+ digitalWrite(LED, HIGH);
+ digitalWrite(LED, LOW);
+
+ // -------------- lidar part --------------------
+ if (distanceSensor.begin() == 0){
+ Serial.println("Lidar online!");
+ Lidar_flag = 1;
+ } else {
+ Lidar_flag = 0;
+ }
+
+ distanceSensor.startRanging();
+ distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+
+ // ----------------- barometer -----------------
+ if (baro.begin()) {
+ Serial.println("Barometer online!");
+ altBaro_flag = 1;
+ } else {
+ altBaro_flag = 0;
+ }
+ baro.setSeaPressure(1016); // STD SLP = 1013.26 hPa
+
+ // --------------------------- esp now ---------------------------
+ // 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 peer
+ 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);
+
+}
+
+// ================================== Main loop ===================================================
+void loop()
+{
+ //-------------------------------------------------------------------------------------
+ // ========== goal finder =========
+ int id = -1;
+ int tx = 0; int ty = 0; int tz = 0;
+ int rx = 0; int ry = 0; int rz = 0;
+ int x = 0;
+ int y = 0;
+ bool goalfind_flag = 0;
+ /*
+ goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz);
+ if (goalfind_flag){
+ Sent_tx = tx;
+ Sent_ty = ty;
+ Sent_tz = tz; // cm
+ Sent_rx = rx;
+ Sent_ry = ry;
+ Sent_rz = rz;
+ } else{
+ Sent_tx = 0;
+ Sent_ty = 0;
+ Sent_tz = 0; // cm
+ Sent_rx = 0;
+ Sent_ry = 0;
+ Sent_rz = 0;
+ } */
+ if (Lidar_flag == 1){
+ Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+ } else {
+ Sent_dist = 0;
+ }
+ if (altBaro_flag == 1){
+ Sent_altBaro = baro.getAltitude(); // Barometer altitude (use the real Barometer data)
+ } else {
+ Sent_altBaro = 0;
+ }
+ Serial.println(altBaro_flag);
+ Serial.println(Sent_altBaro);
+ // ========== lidar state info =========
+ if (Sent_dist < 3000 && Sent_dist > 0){
+ receivedData.DebugM = "altLidar";
+ }else{
+ receivedData.DebugM = "altBaro";
+ }
+ // ========== send info =========
+ receivedData.Rtx = Sent_tx;
+ receivedData.Rty = Sent_ty;
+ receivedData.Rtz = Sent_tz;
+ receivedData.Rrx = Sent_rx;
+ receivedData.Rry = Sent_ry;
+ receivedData.Rrz = Sent_rz;
+ receivedData.Rdist = Sent_dist;
+ receivedData.RaltBaro = Sent_altBaro;
+ send_var_once();
+ // send_message();
+ print_received_Data();
+
+ //-------------------------------------------------------------------------------------
+ // control_motion();
+ //-------------------------------------------------------------------------------------
+}
+// ================================== ^ Main loop ^ ===================================================
+void control_motion(){
+ // vertical motor
+ motorVertical_L->setSpeed(abs(Rec_pwm1));
+ motorVertical_R->setSpeed(abs(Rec_pwm2));
+
+ if (Rec_dir1 == "+"){
+ motorVertical_L->run(BACKWARD); // up
+ }else if (Rec_dir1 == "-"){
+ motorVertical_L->run(FORWARD); // down
+ }
+
+ if (Rec_dir2 == "+"){
+ motorVertical_R->run(FORWARD);
+ }else if (Rec_dir2 == "-"){
+ motorVertical_R->run(BACKWARD);
+ }
+
+ // horizontal motor
+ motorLeft->setSpeed(abs(Rec_pwm3));
+ motorRight->setSpeed(abs(Rec_pwm4));
+
+ if (Rec_dir3 == "+"){
+ motorLeft->run(BACKWARD); // make it move forward
+ }else if (Rec_dir3 == "-"){
+ motorLeft->run(FORWARD); // make it move backward
+ }
+
+ if (Rec_dir4 == "+"){
+ motorRight-> run(FORWARD); // make it move forward
+ }else if (Rec_dir4 == "-"){
+ motorRight-> run(BACKWARD); // make it move backward
+ }
+
+}
+
+void print_received_Data(){
+ if (print_count == 0){
+ Serial.print("Rec_pwm1:");
+ Serial.println(Rec_pwm1);
+ Serial.print("Rec_pwm2:");
+ Serial.println(Rec_pwm2);
+ Serial.print("Rec_pwm3:");
+ Serial.println(Rec_pwm3);
+ Serial.print("Rec_pwm4:");
+ Serial.println(Rec_pwm4);
+ Serial.print("Rec_dir1:");
+ Serial.println(Rec_dir1);
+ Serial.print("Rec_dir2:");
+ Serial.println(Rec_dir2);
+ Serial.print("Rec_dir3:");
+ Serial.println(Rec_dir3);
+ Serial.print("Rec_dir4:");
+ Serial.println(Rec_dir4);
+ Serial.println("_______________________");
+ }
+ print_count +=1;
+}
+
+void send_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(50); // delay 50 ms after send out the message
+}
diff --git a/Code/BaroLIDAR_Altitude_Code/ESP32_slave/utilities.cpp b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/utilities.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a90e33a9ea7f73c7ad525b0919f9c69b4928fda
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/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/BaroLIDAR_Altitude_Code/ESP32_slave/utilities.h b/Code/BaroLIDAR_Altitude_Code/ESP32_slave/utilities.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1ad9adbad7124479fd8d5168a7b1b24a915521c
--- /dev/null
+++ b/Code/BaroLIDAR_Altitude_Code/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
diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index aa4848ae0b9e4e0007e7932f7b4c56e68ff5102e..f92dc8c3acb7632afec698fef733d36153d970b6 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -12,14 +12,14 @@
// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
// MAC Address of the receiver (MASTER)
-uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
-// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
String success;
// Define variables to store incoming readings
@@ -48,6 +48,7 @@ int Sent_rx = 0;
int Sent_ry = 0;
int Sent_rz = 0;
int Sent_dist = 0;
+float Sent_baro = 0;
int8_t g1 = 0,g2=1,g3=2;
int8_t goal_id[3] = {g1, g2, g3};
@@ -59,6 +60,7 @@ int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
// Define Barometer variables
Adafruit_MPL3115A2 baro;
+float groundAlt = 0.0;
// Define LED variable
int LED = LED_BUILTIN;
@@ -74,7 +76,7 @@ typedef struct struct_message {
int Rry;
int Rrz;
int Rdist;
- int Rbaro;
+ float Rbaro;
String DebugM;
int Spwm1;
int Spwm2;
@@ -118,7 +120,7 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Rec_dir4 = receivedData.Sdir4;
count_var = 0;
count = 0;
- print_count=0;
+ print_count = 0;
change_count = 0;
change_count = 0;
}
@@ -165,6 +167,7 @@ void setup() {
Serial.println("Barometer online!");
Baro_flag = 1;
baro.setSeaPressure(1016); // STD SLP = 1013.26 hPa
+ groundAlt = baro.getAltitude();
} else {
Baro_flag = 0;
}
@@ -228,14 +231,14 @@ void loop()
Sent_rz = 0;
}
if (Lidar_flag == 1){
- Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+ Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
} else {
Sent_dist = 0;
}
if (Baro_flag == 1){
- Sent_baro = baro.getAltitude(); // Barometer altitude (use the real Barometer data)
+ Sent_baro = baro.getAltitude() - groundAlt; // Barometer altitude (use the real Barometer data)
} else {
- Sent_baro = 0;
+ Sent_baro = 0.0;
}
// ========== lidar state info =========
if (Sent_dist < 300 && Sent_dist > 0){
@@ -256,7 +259,7 @@ void loop()
print_received_Data();
//-------------------------------------------------------------------------------------
- control_motion();
+// control_motion();
//-------------------------------------------------------------------------------------
}
// ================================== ^ Main loop ^ ===================================================
@@ -297,23 +300,23 @@ void control_motion(){
void print_received_Data(){
if (print_count == 0){
- Serial.print("Rec_pwm1:");
- Serial.println(Rec_pwm1);
- Serial.print("Rec_pwm2:");
- Serial.println(Rec_pwm2);
- Serial.print("Rec_pwm3:");
- Serial.println(Rec_pwm3);
- Serial.print("Rec_pwm4:");
- Serial.println(Rec_pwm4);
- Serial.print("Rec_dir1:");
- Serial.println(Rec_dir1);
- Serial.print("Rec_dir2:");
- Serial.println(Rec_dir2);
- Serial.print("Rec_dir3:");
- Serial.println(Rec_dir3);
- Serial.print("Rec_dir4:");
- Serial.println(Rec_dir4);
-Serial.println("_______________________");
+ Serial.print("Rec_pwm1:");
+ Serial.println(Rec_pwm1);
+ Serial.print("Rec_pwm2:");
+ Serial.println(Rec_pwm2);
+ Serial.print("Rec_pwm3:");
+ Serial.println(Rec_pwm3);
+ Serial.print("Rec_pwm4:");
+ Serial.println(Rec_pwm4);
+ Serial.print("Rec_dir1:");
+ Serial.println(Rec_dir1);
+ Serial.print("Rec_dir2:");
+ Serial.println(Rec_dir2);
+ Serial.print("Rec_dir3:");
+ Serial.println(Rec_dir3);
+ Serial.print("Rec_dir4:");
+ Serial.println(Rec_dir4);
+ Serial.println("_______________________");
}
print_count +=1;
}
diff --git a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
index c2753856cb166c114ced662fde44f680dcbc6b66..6d7f2974936408d1e52630a8f5edcec16a658904 100644
--- a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
+++ b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
@@ -30,7 +30,7 @@ typedef struct struct_message {
int Rry;
int Rrz;
int Rdist;
- int Rbaro;
+ float Rbaro;
String DebugM;
int Spwm1;
int Spwm2;