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 446b2506f8497ee6d91e1e8a121d0b4f0ffc28cb..957a550939007be375f9ce8bb5020d8825e336fe 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 c32e3b41b30ed8d5dc0e58491546bf190020a22c..38d067a1ceefe260a08556b550f00187534ba91d 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));
diff --git a/Code/Control/Laptop_Code/Two_Lidars_comm_test/Sunday1031_Two_Lidar_Test/Sunday1031_Two_Lidar_Test.ino b/Code/Control/Laptop_Code/Two_Lidars_comm_test/Sunday1031_Two_Lidar_Test/Sunday1031_Two_Lidar_Test.ino
new file mode 100644
index 0000000000000000000000000000000000000000..df1b619b7f60f138deec376656d721abbc54c07a
--- /dev/null
+++ b/Code/Control/Laptop_Code/Two_Lidars_comm_test/Sunday1031_Two_Lidar_Test/Sunday1031_Two_Lidar_Test.ino
@@ -0,0 +1,87 @@
+#include <Wire.h>
+#include <VL53L1X.h>
+
+#define XSHUT_pin1 A7
+#define XSHUT_pin2 A6
+
+
+//ADDRESS_DEFAULT 0b0101001 or 41
+#define Sensor1_newAddress 43
+#define Sensor2_newAddress 45
+
+
+VL53L1X Sensor1;
+VL53L1X Sensor2;
+
+
+void setup()
+{
+
+ pinMode(XSHUT_pin1, OUTPUT);
+ pinMode(XSHUT_pin2, OUTPUT);
+
+
+ Serial.begin(115200);
+
+ Wire.begin();
+ Wire.setClock(100000);
+
+
+ pinMode(XSHUT_pin1, INPUT);
+ if (!Sensor1.init())
+ {
+ Serial.println("Failed to detect and initialize sensor1!");
+ while (1);
+ }
+ delay(10);
+ Sensor1.setAddress(Sensor1_newAddress);
+
+ pinMode(XSHUT_pin2, INPUT);
+ if (!Sensor2.init())
+ {
+ Serial.println("Failed to detect and initialize sensor2!");
+ while (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);
+
+}
+
+
+void loop()
+{
+ Sensor1.startContinuous(50);
+ Sensor1.read();
+ Sensor1.stopContinuous();
+ long time1 = millis();
+ Sensor2.startContinuous(50);
+ Sensor2.read();
+ Sensor2.stopContinuous();
+ long time2 = millis();
+
+
+ Serial.print("Sensor 1: ");
+ Serial.print(Sensor1.ranging_data.range_mm);
+ Serial.print(" ");
+ Serial.print("mm \t Sensor 2: ");
+ Serial.print(Sensor2.ranging_data.range_mm);
+ Serial.print(" ");
+ Serial.print("mm \t Time: ");
+ Serial.print(time2-time1);
+
+ Serial.println();
+
+}
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 69101c8154a2c6388175c90c40aeb2a29811f974..553a0b29ecd1f987cbade2826e7a0d32534cb994 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -25,13 +25,13 @@ def serial_port_in_v1(serial_port):
serial_port : serial.Serail object
Output:
- tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
+ tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, DebugM
'''
# DEBUG Verbose
print("initiating one round of serial in ...")
- for i in range(7):
+ for i in range(8):
line = serial_port.readline()
val = int(line.decode())
@@ -48,7 +48,9 @@ def serial_port_in_v1(serial_port):
elif i == 5:
rz = val
elif i == 6:
- LIDAR_dist = val
+ LIDAR_dist1 = val
+ elif i == 7:
+ LIDAR_dist2 = val
line = serial_port.readline()
debugM = line.decode()
@@ -60,10 +62,11 @@ def serial_port_in_v1(serial_port):
print("rx:{}".format(rx))
print("ry:{}".format(ry))
print("rz:{}".format(rz))
- print("dist:{}".format(LIDAR_dist))
+ print("LIDAR_dist1:{}".format(LIDAR_dist1))
+ print("LIDAR_dist2:{}".format(LIDAR_dist2))
print(debugM)
- return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
+ return tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM
def serial_port_in_v2(serial_port):
'''
@@ -74,24 +77,28 @@ def serial_port_in_v2(serial_port):
serial_port : serial.Serail object
Output:
- LIDAR_dist,DebugM
+ LIDAR_dist1, LIDAR_dist2, DebugM
'''
# debug info
print("initiating serial in V2...")
- for i in range(2):
+ for i in range(3):
line = serial_port.readline()
if i == 0:
val = int(line.decode())
- LIDAR_dist = val
+ LIDAR_dist1 = val
elif i == 1:
+ val = int(line.decode())
+ LIDAR_dist2 = val
+ elif i == 2:
debugM = line.decode()
- print("Lidar_dist:{}".format(LIDAR_dist))
+ print("LIDAR_dist1:{}".format(LIDAR_dist1))
+ print("LIDAR_dist2:{}".format(LIDAR_dist2))
print(debugM)
- return LIDAR_dist, debugM
+ return LIDAR_dist1, LIDAR_dist2, debugM
def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
'''
@@ -142,11 +149,11 @@ def goal_detect(tx,ty):
else:
return True
-def ball_capture(LIDAR_dist):
+def ball_capture(LIDAR_dist1):
'''
return True if April Tag is detected
'''
- if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
+ if (LIDAR_dist1 < LIDAR_Thres) and (LIDAR_dist1 > 0): # Ball captured
return True
else:
return False
@@ -162,7 +169,7 @@ def stop_all():
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-def move2goal(tx, ty):
+def move2goal(tx, ty,tz):
"""
Description:
Given the center of the AT tx, ty. Call PID control to output the blimp
@@ -176,13 +183,24 @@ def move2goal(tx, ty):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
- inputx = tx / 1.00
- inputy = -1.00 * (ty + AT_goal_Delta) / 1.00 #
+
+
# April Tag Center
setpoint_x1 = 0.0
setpoint_y1 = 0.0
+ if tz <200:
+ kpy_g,kiy_g,kdy_g = 0.0, 0.0, 0.0
+ base_speed = 150
+ AT_goal_Delta = 150
+ else:
+ kpy_g,kiy_g,kdy_g = 2, 0.01, 0.5
+ base_speed = 120
+ AT_goal_Delta = 0
+ inputx = tx / 1.00
+ inputy = (ty + AT_goal_Delta) / 1.00 #
+
pid_x = PID(kpx_g, kix_g, kdx_g, setpoint = setpoint_x1)
pid_y = PID(kpy_g, kiy_g, kdy_g, setpoint = setpoint_y1)
@@ -361,7 +379,7 @@ def move2ball(gbx, gby, gb_dist):
# =========== Main Control ===========
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h):
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h):
'''
Description:
Given green ball information and AT information, the main control logic
@@ -369,7 +387,8 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,c
Input:
gbx, gby, gb_dist : green ball information
- tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
+ tx, ty, tz, rx, ry, rz, : AirTag information
+ LIDAR_dist1, LIDAR_dist2 : Lidar info
debugM : Debug Message
Output:
@@ -380,7 +399,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,c
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist)
+ ballCapture = ball_capture(LIDAR_dist1)
goalDetect = goal_detect(tx, ty)
ballCapture = 1
@@ -391,7 +410,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,c
if ballCapture: # Ball captured
if goalDetect: # Goal detected
# stop_all() # Debug
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty,tz)
else: # Goal not detected
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
@@ -405,14 +424,14 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,c
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h):
+def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h):
# ===== STEP 1: TAKE ALL INPUT =====
# gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
line = serial_port.readline()
if openmv_on == 1:
if line == b'SERIAL_IN_START\r\n':
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in_v1(serial_port)
+ tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM = serial_port_in_v1(serial_port)
print("gbx,gby:{},{}".format(gbx, gby))
time.sleep(waitTime)
@@ -422,21 +441,21 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
# ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist,
- debugM,count_h)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2,
+ debugM, count_h)
# ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
time.sleep(waitTime)
-def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
-
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM):
+ count_h = 0
# gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
init_count += 1
- count_h = 0
+
return init_count,count_h
def init():
@@ -600,7 +619,7 @@ def test_function():
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
+ PORT = "COM9" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -609,7 +628,8 @@ if __name__ == '__main__':
waitTime = 0.05
# Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ if ml == 1:
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES ===========
# ESP CAM In
@@ -619,7 +639,8 @@ if __name__ == '__main__':
# Serial Port In
tx, ty, tz = 100000, 100000, 100000 # by default (0 means no found AirTag)
rx, ry, rz = 0, 0, 0
- LIDAR_dist = 0
+ LIDAR_dist1 = 0
+ LIDAR_dist2 = 0
debugM = 'Testing'
# Serial Port Out
@@ -629,7 +650,7 @@ if __name__ == '__main__':
# Trigger the ESP32_SLAVE to talk first
"""
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
"""
flag = 0
@@ -637,7 +658,7 @@ if __name__ == '__main__':
print_count = 0
global start_speed
start_speed = 70
-
+ count_h = 0
init()
# =========== LOOP FOREVER===========
while True:
@@ -645,8 +666,8 @@ if __name__ == '__main__':
flag = 1
while (flag == 1):
if init_count == 0:
- init_count,count_h = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h)
+ init_count,count_h = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
+ auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h)
flag, print_count = keyboard_stop(flag,print_count)
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()