diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index f79c97c83061e6cf1f75ad7859c7af98c83b4f03..d036e5ce91eb2065086a2c5753770b2fc93b7247 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -1,20 +1,27 @@
#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>
-#include <SparkFun_VL53L1X.h>
+#include <VL53L1X.h>
#include <Arduino.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)
-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, 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
@@ -35,9 +42,10 @@ 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;
+
// Define variables to be sent;
int Sent_tx = 0;
int Sent_ty = 0;
@@ -46,12 +54,17 @@ int Sent_rx = 0;
int Sent_ry = 0;
int Sent_rz = 0;
int Sent_dist = 0;
+int Sent_dist1 = 0;
+int Sent_dist2 = 0;
int8_t g1 = 0,g2=1,g3=2;
int8_t goal_id[3] = {g1, g2, g3};
// Define Lidar variables
-SFEVL53L1X distanceSensor;
+// Define Lidar variables
+VL53L1X Sensor1;
+VL53L1X Sensor2;
+
int budgetIndex = 4;
int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
@@ -68,7 +81,8 @@ typedef struct struct_message {
int Rrx;
int Rry;
int Rrz;
- int Rdist;
+ int Rdist1;
+ int Rdist2;
String DebugM;
int Spwm1;
int Spwm2;
@@ -119,8 +133,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
// =============================== 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
+// 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
@@ -135,23 +149,46 @@ void setup() {
// Init Serial Monitor
Serial.begin(115200);
Wire.begin();
- interface.begin(); //communication between ESP and OpenMV
+ // 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("Sensor online!");
- Lidar_flag = 1;
- }else {
- Lidar_flag = 0;
- }
-
- distanceSensor.startRanging();
- distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+
+ // -------------- lidar part --------------------
+ pinMode(XSHUT_pin1, OUTPUT);
+ pinMode(XSHUT_pin2, OUTPUT);
+
+ 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);
+ Sensor1.setTimeout(500);
+ Sensor1.setDistanceMode(VL53L1X::Medium);
+ Sensor1.setMeasurementTimingBudget(33000);
+ }
+
+
+ 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);
+ Sensor2.setTimeout(500);
+ Sensor2.setDistanceMode(VL53L1X::Medium);
+ Sensor2.setMeasurementTimingBudget(33000);
+ }
// --------------------------- esp now ---------------------------
// Set device as a Wi-Fi Station
@@ -211,11 +248,22 @@ void loop()
Sent_ry = 0;
Sent_rz = 0;
}
- if (Lidar_flag == 1){
- Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+ 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_dist = 0;
- }
+ 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;
+ }
// ========== lidar state info =========
if (Sent_dist < 300 && Sent_dist > 0){
receivedData.DebugM = "found b";
@@ -229,7 +277,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;
send_var_once();
print_received_Data();