From 13aeba8748a50e60c3239e9d04b8b8f9d8bf44b2 Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Wed, 10 Nov 2021 15:38:24 -0800
Subject: [PATCH] add some delay mechanism into our move2goal function
---
Code/Control/Laptop_Code/main_keyboard.py | 48 ++++++++++++++++++++---
1 file changed, 42 insertions(+), 6 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 838ec18..6b91915 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -9,7 +9,9 @@ from ESP32_AT.imageTread_AT import get_AT_6DOF_info
import cv2
global ml,esp_cam_on,openmv_on
+outputx, outputy = 0, 0
AT_detected_time = time.time()
+ATDetectActionDelay = 1.0
if ml == 1:
import ball_detection.ball_detection as ball_detection
@@ -183,6 +185,7 @@ def move2goal(tx, ty,tz):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
+ global outputx, outputy, AT_detected_time
# April Tag Center
setpoint_x1 = 0.0
@@ -191,7 +194,7 @@ def move2goal(tx, ty,tz):
if tz < 2.0:
kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
base_speed = 200
- AT_goal_Delta = -150
+ AT_goal_Delta = -140
else:
kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
# kpy_g,kiy_g,kdy_g = 0.0, 0.00, 0.0
@@ -208,8 +211,13 @@ def move2goal(tx, ty,tz):
pid_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255)
- outputx = pid_x(inputx)
- outputy = pid_y(inputy)
+ if AT_detect(tx, ty):
+ AT_detected_time = time.time()
+ outputx = pid_x(inputx)
+ outputy = pid_y(inputy)
+
+ print('Delay Debug:\t\t', AT_detected_time, '\t\t', time.time() > AT_detected_time + ATDetectActionDelay)
+
print("outputy:{}".format(outputy))
# Vertical
pwm1 = abs(outputy)
@@ -356,6 +364,8 @@ def move2ball(gbx, gby, gb_dist):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
+ global outputx, outputy
+
inputx = gbx / 1.00
inputy = gby / 1.00
@@ -440,7 +450,16 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
# ballDetect = 0
if ballCapture: # Ball captured
print('ballCapture TRUE')
- if goalDetect: # Goal detected
+ if esp_cam_on == 1:
+
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
+ #LIDAR_dist1 = 0
+ # LIDAR_dist2 = 0
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+
+ debugM = "using two esp32 cam"
+ goalDetect = goal_detect(tx, ty)
+ if goalDetect or time.time() < AT_detected_time + ATDetectActionDelay: # Goal detected or not too long since it was detected
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty,tz)
else: # Goal not detected
@@ -448,6 +467,11 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
else: # Ball not captured
print('ballCapture FALSE')
+ if ml ==1:
+ gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
+
+ ballDetect = ball_detect(gbx, gby)
+
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
@@ -461,8 +485,11 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h,bcap_man):
# ===== STEP 1: TAKE ALL INPUT =====
# print('in auto_control')
+ """
if ml ==1:
gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
+ """
+
line = serial_port.readline()
# print('auto')
if openmv_on == 1:
@@ -471,14 +498,17 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
print("gbx,gby:{},{}".format(gbx, gby))
time.sleep(waitTime)
-
+ """
if esp_cam_on == 1:
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
#LIDAR_dist1 = 0
# LIDAR_dist2 = 0
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+
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,
@@ -573,6 +603,12 @@ def dynamic_variable(str_name_v):
elif str_name_v == "kdy_g":
kdx = input("Enter your value: ")
print("kdy_g:{}".format(kdy_g))
+ elif str_name_v == "ssp":
+ seeking_speed = input("Enter your value: ")
+ print("seeking_speed:{}".format(seeking_speed))
+ elif str_name_v == "bsp":
+ base_speed = input("Enter your value: ")
+ print("base_speed:{}".format(base_speed))
def variables_change_once():
@@ -694,7 +730,7 @@ def manual_ballcapture(bcap_man):
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM9" # for Alienware
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
--
GitLab