From 663140e224f68b957b44c025339c3069340570fd Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Thu, 4 Nov 2021 22:23:18 -0700
Subject: [PATCH] change the move2goal function based on the field test
---
Code/Control/Laptop_Code/main_keyboard.py | 116 ++++++++++++++++------
1 file changed, 85 insertions(+), 31 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index e709ad5..d143639 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -8,9 +8,11 @@ from constants import *
from ESP32_AT.imageTread_AT import get_AT_6DOF_info
global ml,esp_cam_on,openmv_on
-ml = 1
-esp_cam_on = 0
-openmv_on = 1
+ml = 0
+esp_cam_on = 1
+openmv_on = 0
+seekVertDir = 1
+AT_detected_time = time.time()
if ml == 1:
import ball_detection.ball_detection as ball_detection
@@ -31,7 +33,7 @@ def serial_port_in_v1(serial_port):
# DEBUG Verbose
print("initiating one round of serial in ...")
- for i in range(8):
+ for i in range(7):
line = serial_port.readline()
val = int(line.decode())
@@ -49,9 +51,10 @@ def serial_port_in_v1(serial_port):
rz = val
elif i == 6:
LIDAR_dist1 = val
- elif i == 7:
- LIDAR_dist2 = val
+ # elif i == 7:
+ # LIDAR_dist2 = val
+ LIDAR_dist2 = 0
line = serial_port.readline()
debugM = line.decode()
@@ -190,16 +193,17 @@ def move2goal(tx, ty,tz):
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
+ if tz < 2.0:
+ kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
+ base_speed = 200
+ AT_goal_Delta = -150
else:
- kpy_g,kiy_g,kdy_g = 2, 0.01, 0.5
- base_speed = 120
+ kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
+ # kpy_g,kiy_g,kdy_g = 0.0, 0.00, 0.0
+ base_speed = 140
AT_goal_Delta = 0
- inputx = tx / 1.00
- inputy = (ty + AT_goal_Delta) / 1.00 #
+ inputx = tx * 100/ 1.00
+ inputy = (ty * 100 + 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)
@@ -211,17 +215,17 @@ def move2goal(tx, ty,tz):
outputx = pid_x(inputx)
outputy = pid_y(inputy)
-
+ print("outputy:{}".format(outputy))
# Vertical
pwm1 = abs(outputy)
pwm2 = abs(outputy)
if(outputy > 0):
- dir1 = '-'
- dir2 = '-'
- else:
dir1 = '+'
dir2 = '+'
+ else:
+ dir1 = '-'
+ dir2 = '-'
# Horizontal
lspeed = -1 * outputx + base_speed
@@ -253,9 +257,9 @@ def ball_seeking(count_h,tx,ty):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
- global set_h
+ global set_h, seekVertDir
if openmv_on == 1:
- delta_h = 100
+ delta_h = 100 # cm
threshold_h = 20
elif esp_cam_on == 1:
delta_h = 2 # meter
@@ -282,6 +286,23 @@ def ball_seeking(count_h,tx,ty):
else:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+
+ # if time.time() > AT_detected_time + AT_detectBreak:
+ # AT_detected = AT_detect(tx, ty)
+ # AT_detected_time = time.time()
+ #
+ # while facingWall():
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+ #
+ # if AT_detected:
+ # current_h = get_altitude_from_AT(AT_h1,ty)
+ #
+ # if LIDAR_dist2 < LIDAR2_Thres:
+ # seekVertDir = 0
+ # if baroHeight < baroThres:
+ # seekVertDir = 1
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move_in_spiral(new_ssp, seekVertDir)
+
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def vertical_control(current_h,set_h):
@@ -317,6 +338,14 @@ def rotate_one_direction_ssp(ssp):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+def move_in_spiral(ssp, dir):
+ pwm1, pwm2, pwm3, pwm4 = ssp, ssp, ssp, ssp/2
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+ if dir == 0:
+ dir1, dir2 = '-', '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
def move2ball(gbx, gby, gb_dist):
"""
Description:
@@ -394,6 +423,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
+ print('in main_control')
# placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
@@ -402,12 +432,14 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
ballCapture = ball_capture(LIDAR_dist1)
goalDetect = goal_detect(tx, ty)
- ballCapture = 1
+ ballCapture = 0
# debug
- # ballCapture = 1
+ ballCapture = 1
# goalDetect = 0
+ # ballDetect = 0
if ballCapture: # Ball captured
+ print('ballCapture TRUE')
if goalDetect: # Goal detected
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty,tz)
@@ -415,21 +447,24 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
else: # Ball not captured
+ print('ballCapture FALSE')
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
# stop_all() # Debug
- # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
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_dist1, LIDAR_dist2, debugM,count_h):
# ===== STEP 1: TAKE ALL INPUT =====
+ # print('in auto_control')
if ml ==1:
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
line = serial_port.readline()
-
+ # print('auto')
if openmv_on == 1:
if line == b'SERIAL_IN_START\r\n':
tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM = serial_port_in_v1(serial_port)
@@ -438,8 +473,12 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
if esp_cam_on == 1:
- url = 'http://192.168.1.118/cam-hi.jpg'
+ url = 'http://192.168.0.204/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ LIDAR_dist1 = 0
+ LIDAR_dist2 = 0
+ debugM = "using two esp32 cam"
+
# ===== 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_dist1, LIDAR_dist2,
@@ -452,7 +491,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
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)
+ # gbx, gby, gb_dist = ball_detection.detectLive(url_gb, 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_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
init_count += 1
@@ -572,7 +611,7 @@ def manual_control(Ctl_com,serial_port):
waitTime = 0.05
# changed
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ # gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -606,8 +645,21 @@ def get_altitude_from_AT(AT_h,ty):
altitude = AT_h - ty
return altitude
+
+
def test_function():
- url = 'http://192.168.1.118/cam-hi.jpg'
+ url = 'http://192.168.0.230/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ print("testing new function")
+ print("-----------------------")
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+
+
+def test_function1():
+ url = 'http://192.168.0.230/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
print("testing new function")
print("-----------------------")
@@ -616,6 +668,7 @@ def test_function():
print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
@@ -651,7 +704,7 @@ if __name__ == '__main__':
Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
# Trigger the ESP32_SLAVE to talk first
"""
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ gbx, gby, gb_dist = ball_detection.detectLive(url_gb, 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_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
"""
@@ -669,6 +722,7 @@ if __name__ == '__main__':
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_dist1, LIDAR_dist2, debugM)
+ print('auto_control')
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'):
--
GitLab