From d3c2200bf201912821729ee3b674a91ced5266ed Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Mon, 8 Nov 2021 12:28:55 -0800
Subject: [PATCH] add keyboard manually determine green ball capture state
function
---
Code/Control/Laptop_Code/main_keyboard.py | 35 ++++++++++++++++-------
1 file changed, 24 insertions(+), 11 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index aeb9e42..838ec18 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -184,8 +184,6 @@ def move2goal(tx, ty,tz):
dir1, dir2, dir3, dir4
"""
-
-
# April Tag Center
setpoint_x1 = 0.0
setpoint_y1 = 0.0
@@ -405,7 +403,7 @@ def move2ball(gbx, gby, gb_dist):
# =========== Main Control ===========
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h):
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h,bcap_man):
'''
Description:
Given green ball information and AT information, the main control logic
@@ -432,8 +430,13 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
ballCapture = 0
# debug
- # ballCapture = 1
- # goalDetect = 0
+ if bcap_man == 1:
+ ballCapture = 1
+ elif bcap_man == 0:
+ ballCapture = 0
+ elif bcap_man == -1:
+ ballCapture = ball_capture(LIDAR_dist1)
+
# ballDetect = 0
if ballCapture: # Ball captured
print('ballCapture TRUE')
@@ -455,7 +458,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
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):
+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:
@@ -479,7 +482,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
# ===== 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,
- debugM, count_h)
+ debugM, count_h,bcap_man)
# ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -488,12 +491,13 @@ 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
+ bcap_man = -1
# 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)
+ 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,bcap_man)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
init_count += 1
- return init_count,count_h
+ return init_count,count_h,bcap_man
def init():
pygame.init()
@@ -678,6 +682,14 @@ def manual_return2auto(key_press,flag_r):
flag_r = 1
return flag_r
+def manual_ballcapture(bcap_man):
+ if get_key('b'):
+ bcap_man = 1
+ elif get_key('n'):
+ bcap_man = 0
+ elif get_key('v'):
+ bcap_man = -1
+ return bcap_man
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
@@ -730,11 +742,12 @@ 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_dist1, LIDAR_dist2, debugM)
+ init_count,count_h,bcap_man = 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)
+ auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h,bcap_man)
flag, print_count = keyboard_stop(flag,print_count)
flag = manual_in_auto(Ctl_com,serial_port,flag)
+ bcap_man = manual_ballcapture(bcap_man)
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
--
GitLab