From f43de3bb203cef7e1e23efc17604edffad6099fe Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Sun, 31 Oct 2021 23:21:40 -0700
Subject: [PATCH] upgrade keyboard with 2 Lidars
---
Code/Control/Laptop_Code/main_keyboard.py | 89 ++++++++++++++---------
1 file changed, 55 insertions(+), 34 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 69101c8..553a0b2 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()
--
GitLab