From c26c25077469d03261893b4fff409b4eee520c88 Mon Sep 17 00:00:00 2001
From: Zhaoliang <zhz03@g.ucla.edu>
Date: Sun, 7 Nov 2021 16:36:35 -0800
Subject: [PATCH] add keyboard manually determine green ball capture state
function
---
Code/Control/Laptop_Code/main_keyboard.py | 37 +++++++++++++++++------
1 file changed, 27 insertions(+), 10 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index b38848b..98281b3 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -408,7 +408,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
@@ -429,14 +429,20 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist1)
+ # ballCapture = ball_capture(LIDAR_dist1)
goalDetect = goal_detect(tx, ty)
ballCapture = 0
# debug
- ballCapture = 1
- # goalDetect = 0
+ if bcap_man == 1:
+ ballCapture = 1 # Manually determine Ball captured
+ elif bcap_man == 0:
+ ballCapture = 0 # Ball not captured
+ elif bcap_man == -1:
+ ballCapture = ball_capture(LIDAR_dist1)
+
+ # goalDetect = 0
# ballDetect = 0
if ballCapture: # Ball captured
print('ballCapture TRUE')
@@ -458,7 +464,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:
@@ -482,7 +488,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)
@@ -491,12 +497,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()
@@ -680,6 +687,14 @@ def manual_return2auto(key_press):
flag_r = 1
return flag_r
+def manual_ballcapture(bcap_man):
+ if get_key('b'): # the green ball is captured
+ bcap_man = 1
+ elif get_key('n'): # no ball is captured
+ bcap_man = 0
+ elif get_key('v'): # visual
+ bcap_man = -1
+ return bcap_man
# ===== Main Function =====
if __name__ == '__main__':
@@ -714,6 +729,7 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+
# Trigger the ESP32_SLAVE to talk first
"""
gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
@@ -733,11 +749,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)
+ bcap_man = manual_ballcapture(bcap_man)
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
--
GitLab