diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index f92dc8c3acb7632afec698fef733d36153d970b6..053b76791e2b4fe5ef0f1cd2cf2f4ad24afc9800 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -2,13 +2,20 @@
#include <Wire.h>
#include <Arduino.h>
#include <esp_now.h>
-#include <SparkFun_VL53L1X.h>
+#include <VL53L1X.h>
#include <Adafruit_MPL3115A2.h>
-
+
#include "Camera.h"
#include "utilities.h"
#include <Adafruit_MotorShield.h>
+// For LIDAR 1 and 2
+#define XSHUT_pin1 A7
+#define XSHUT_pin2 A6
+
+#define Sensor1_newAddress 43
+#define Sensor2_newAddress 45
+
// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
// MAC Address of the receiver (MASTER)
@@ -36,8 +43,9 @@ String Rec_dir4;
int count = 0;
int count_var = 0;
int print_count = 0;
-int change_count =0;
-int Lidar_flag = 0;
+int change_count = 0;
+int Lidar1_flag = 0;
+int Lidar2_flag = 0;
int Baro_flag = 0;
// Define variables to be sent;
@@ -47,16 +55,16 @@ int Sent_tz = 0;
int Sent_rx = 0;
int Sent_ry = 0;
int Sent_rz = 0;
-int Sent_dist = 0;
+int Sent_dist1 = 0;
+int Sent_dist2 = 0;
float Sent_baro = 0;
-int8_t g1 = 0,g2=1,g3=2;
+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};
+VL53L1X Sensor1;
+VL53L1X Sensor2;
// Define Barometer variables
Adafruit_MPL3115A2 baro;
@@ -68,25 +76,26 @@ 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;
+typedef struct struct_message {
+ int Rtx;
+ int Rty;
+ int Rtz;
+ int Rrx;
+ int Rry;
+ int Rrz;
+ int Rdist1;
+ int Rdist2;
float Rbaro;
- String DebugM;
- int Spwm1;
- int Spwm2;
- int Spwm3;
- int Spwm4;
- String Sdir1;
- String Sdir2;
- String Sdir3;
- String Sdir4;
-} struct_message;
+ 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;
@@ -133,7 +142,7 @@ 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
@@ -152,16 +161,40 @@ void setup() {
digitalWrite(LED, LOW);
// -------------- lidar part --------------------
- if (distanceSensor.begin() == 0){
- Serial.println("Lidar online!");
- Lidar_flag = 1;
- } else {
- Lidar_flag = 0;
- }
+ pinMode(XSHUT_pin1, OUTPUT);
+ pinMode(XSHUT_pin2, OUTPUT);
- distanceSensor.startRanging();
- distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+ Wire.setClock(100000);
+
+ pinMode(XSHUT_pin1, INPUT);
+ if (!Sensor1.init()){
+ Serial.println("Failed to detect and initialize sensor1!");
+ Lidar1_flag = 0;
+ } else {
+ Lidar1_flag = 1;
+ }
+ delay(10);
+ Sensor1.setAddress(Sensor1_newAddress);
+ pinMode(XSHUT_pin2, INPUT);
+ if (!Sensor2.init())
+ {
+ Serial.println("Failed to detect and initialize sensor2!");
+ Lidar2_flag = 0;
+ } else {
+ Lidar2_flag = 1;
+ }
+ delay(10);
+ Sensor2.setAddress(Sensor2_newAddress);
+
+ Sensor1.setTimeout(500);
+ Sensor2.setTimeout(500);
+
+ Sensor1.setDistanceMode(VL53L1X::Medium);
+ Sensor1.setMeasurementTimingBudget(33000);
+ Sensor2.setDistanceMode(VL53L1X::Medium);
+ Sensor2.setMeasurementTimingBudget(33000);
+
// ----------------- barometer -----------------
if (baro.begin()) {
Serial.println("Barometer online!");
@@ -199,12 +232,11 @@ void setup() {
}
// Register for a callback function that will be called when data is received
esp_now_register_recv_cb(OnDataRecv);
-
}
// ================================== Main loop ===================================================
-void loop()
-{
+void loop()
+{
//-------------------------------------------------------------------------------------
// ========== goal finder =========
int id = -1;
@@ -230,18 +262,31 @@ void loop()
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 (Lidar1_flag == 1){
+ Sensor1.startContinuous(50);
+ Sensor1.read();
+ Sensor1.stopContinuous();
+ Sent_dist1 = Sensor1.ranging_data.range_mm; // Lidar distance (use the real Lidar data)
+ }else {
+ Sent_dist1 = 0;
}
+ if (Lidar2_flag == 1){
+ Sensor2.startContinuous(50);
+ Sensor2.read();
+ Sensor2.stopContinuous();
+ Sent_dist2 = Sensor2.ranging_data.range_mm; // Lidar distance (use the real Lidar data)
+ }else {
+ Sent_dist2 = 0;
+ }
+
if (Baro_flag == 1){
Sent_baro = baro.getAltitude() - groundAlt; // Barometer altitude (use the real Barometer data)
} else {
Sent_baro = 0.0;
}
+
// ========== lidar state info =========
- if (Sent_dist < 300 && Sent_dist > 0){
+ if (Sent_dist1 < 300 && Sent_dist1 > 0){
receivedData.DebugM = "found b";
}else{
receivedData.DebugM = "no b";
@@ -253,7 +298,8 @@ void loop()
receivedData.Rrx = Sent_rx;
receivedData.Rry = Sent_ry;
receivedData.Rrz = Sent_rz;
- receivedData.Rdist = Sent_dist;
+ receivedData.Rdist1 = Sent_dist1;
+ receivedData.Rdist2 = Sent_dist2;
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 6d7f2974936408d1e52630a8f5edcec16a658904..23a8df10c99494fe17c41a815ae4f46214b42549 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
@@ -29,7 +29,8 @@ typedef struct struct_message {
int Rrx;
int Rry;
int Rrz;
- int Rdist;
+ int Rdist1;
+ int Rdist2;
float Rbaro;
String DebugM;
int Spwm1;
@@ -71,7 +72,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Serial.println(sentData.Rrx);
Serial.println(sentData.Rry);
Serial.println(sentData.Rrz);
- Serial.println(sentData.Rdist);
+ Serial.println(sentData.Rdist1);
+ Serial.println(sentData.Rdist2);
Serial.println(sentData.Rbaro);
Serial.println(sentData.DebugM);
}