diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino index c32e3b41b30ed8d5dc0e58491546bf190020a22c..aa4848ae0b9e4e0007e7932f7b4c56e68ff5102e 100644 --- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino +++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino @@ -1,8 +1,9 @@ -#include <esp_now.h> #include <WiFi.h> #include <Wire.h> -#include <SparkFun_VL53L1X.h> #include <Arduino.h> +#include <esp_now.h> +#include <SparkFun_VL53L1X.h> +#include <Adafruit_MPL3115A2.h> #include "Camera.h" #include "utilities.h" @@ -37,6 +38,7 @@ int count_var = 0; int print_count = 0; int change_count =0; int Lidar_flag = 0; +int Baro_flag = 0; // Define variables to be sent; int Sent_tx = 0; @@ -54,7 +56,10 @@ int8_t goal_id[3] = {g1, g2, g3}; 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; @@ -68,7 +73,8 @@ typedef struct struct_message { int Rrx; int Rry; int Rrz; - int Rdist; + int Rdist; + int Rbaro; String DebugM; int Spwm1; int Spwm2; @@ -114,7 +120,7 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { count = 0; print_count=0; change_count = 0; - change_count = 0; + change_count = 0; } // =============================== All the setup =========================================== @@ -126,15 +132,16 @@ Camera cam(&interface); // 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 +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(); + Serial.begin(115200); + Wire.begin(); interface.begin(); //communication between ESP and OpenMV AFMS.begin(); // create with the default frequency 1.6KHz // -------------- LED part -------------------- @@ -144,16 +151,25 @@ void setup() { // -------------- lidar part -------------------- if (distanceSensor.begin() == 0){ - Serial.println("Sensor online!"); + Serial.println("Lidar online!"); Lidar_flag = 1; - }else { + } else { Lidar_flag = 0; } - + distanceSensor.startRanging(); distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]); - - // --------------------------- esp now --------------------------- + + // ----------------- barometer ----------------- + if (baro.begin()) { + Serial.println("Barometer online!"); + Baro_flag = 1; + baro.setSeaPressure(1016); // STD SLP = 1013.26 hPa + } else { + Baro_flag = 0; + } + + // --------------------------- esp now --------------------------- // Set device as a Wi-Fi Station WiFi.mode(WIFI_STA); @@ -203,7 +219,7 @@ void loop() Sent_rx = rx; Sent_ry = ry; Sent_rz = rz; - }else{ + } else{ Sent_tx = 0; Sent_ty = 0; Sent_tz = 0; // cm @@ -213,8 +229,13 @@ void loop() } if (Lidar_flag == 1){ Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data) - }else { + } else { Sent_dist = 0; + } + if (Baro_flag == 1){ + Sent_baro = baro.getAltitude(); // Barometer altitude (use the real Barometer data) + } else { + Sent_baro = 0; } // ========== lidar state info ========= if (Sent_dist < 300 && Sent_dist > 0){ @@ -230,6 +251,7 @@ void loop() receivedData.Rry = Sent_ry; receivedData.Rrz = Sent_rz; receivedData.Rdist = Sent_dist; + receivedData.Rbaro = Sent_baro; send_var_once(); print_received_Data(); 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 446b2506f8497ee6d91e1e8a121d0b4f0ffc28cb..c2753856cb166c114ced662fde44f680dcbc6b66 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,6 +30,7 @@ typedef struct struct_message { int Rry; int Rrz; int Rdist; + int Rbaro; String DebugM; int Spwm1; int Spwm2; @@ -71,6 +72,7 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { Serial.println(sentData.Rry); Serial.println(sentData.Rrz); Serial.println(sentData.Rdist); + Serial.println(sentData.Rbaro); Serial.println(sentData.DebugM); } // =================================== setup ==================================================