From 31c06094e3b70a7b2a50cf9b3d4489e7466222f6 Mon Sep 17 00:00:00 2001
From: SZL <zhiyingli@g.ucla.edu>
Date: Sun, 31 Oct 2021 22:38:35 -0700
Subject: [PATCH] Update Master and Slave to incorporate with 2 lidars
---
.../ESP32_master/ESP32_master.ino | 6 +-
.../ESP32_slave/ESP32_slave.ino | 94 ++++++++++++++-----
2 files changed, 76 insertions(+), 24 deletions(-)
diff --git a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino
index 446b250..957a550 100644
--- a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_master/ESP32_master.ino
+++ b/Code/Control/Laptop_Code/Two_Lidars_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;
String DebugM;
int Spwm1;
int Spwm2;
@@ -70,7 +71,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.DebugM);
}
// =================================== setup ==================================================
diff --git a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
index c32e3b4..38d067a 100644
--- a/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/Two_Lidars_comm_test/ESP32_slave/ESP32_slave.ino
@@ -1,12 +1,21 @@
#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
@@ -36,7 +45,8 @@ int count = 0;
int count_var = 0;
int print_count = 0;
int change_count =0;
-int Lidar_flag = 0;
+int Lidar1_flag = 0;
+int Lidar2_flag = 0;
// Define variables to be sent;
int Sent_tx = 0;
@@ -45,15 +55,15 @@ 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;
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 LED variable
int LED = LED_BUILTIN;
@@ -68,7 +78,8 @@ typedef struct struct_message {
int Rrx;
int Rry;
int Rrz;
- int Rdist;
+ int Rdist1;
+ int Rdist2;
String DebugM;
int Spwm1;
int Spwm2;
@@ -143,15 +154,41 @@ void setup() {
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]);
+
+ 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);
+
+ 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);
+
// --------------------------- esp now ---------------------------
// Set device as a Wi-Fi Station
@@ -211,13 +248,24 @@ 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){
+ if (Sent_dist1 < 300 && Sent_dist1 > 0){
receivedData.DebugM = "found b";
}else{
receivedData.DebugM = "no 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();
@@ -238,6 +287,7 @@ void loop()
//-------------------------------------------------------------------------------------
}
// ================================== ^ Main loop ^ ===================================================
+
void control_motion(){
// vertical motor
motorVertical_L->setSpeed(abs(Rec_pwm1));
--
GitLab