From 8b82de427477cb6ee33726d5f7cc81caae69dd4c Mon Sep 17 00:00:00 2001
From: Zhaoliang <zhz03@g.ucla.edu>
Date: Sun, 24 Oct 2021 17:19:11 -0700
Subject: [PATCH] upload the main keyboard logic
---
Code/Control/Laptop_Code/main_keyboard.py | 75 +++++++++++++++++------
1 file changed, 56 insertions(+), 19 deletions(-)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index b335c2b..7330c94 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -301,6 +301,13 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
# ============= keyboard interruption ===================
+def keyboard_stop():
+ key = cv2.waitKey(5)
+ if key == ord('q'):
+ flag = 0
+ print_count = 1
+ return flag,print_count
+
def keyboard_interrupt(key):
if key == ord('s'):
@@ -310,6 +317,28 @@ def keyboard_interrupt(key):
elif key == ord('v'):
variables_change()
+def auto_control():
+ # =================================== tested autonomous control ======================================================
+ # ===== STEP 1: TAKE ALL INPUT =====
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight=True)
+ line = serial_port.readline()
+
+ if line == b'SERIAL_IN_START\r\n':
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+ print("gbx,gby:{},{}".format(gbx, gby))
+ time.sleep(waitTime)
+
+ # ===== 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)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime) # second
+ # =========================================================================================
+
+
def manual_control():
pass
@@ -351,30 +380,38 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ flag = 0
+ print_count = 1
# =========== LOOP FOREVER===========
while True:
- # ===== STEP 1: TAKE ALL INPUT =====
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- line = serial_port.readline()
-
- if line == b'SERIAL_IN_START\r\n':
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
- print("gbx,gby:{},{}".format(gbx,gby))
- time.sleep(waitTime)
-
- # ===== 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)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- time.sleep(waitTime) # second
key = cv2.waitKey(5)
- keyboard_interrupt(key)
-
-
+ if key == ord('a'):
+ flag = 1
+ while (flag == 1):
+ auto_control()
+ flag,print_count = keyboard_stop()
+
+ elif key == ord('s'):
+ stop_all()
+ print("stop all motors")
+
+ elif key == ord('c'):
+ flag = 2
+ while (flag == 2):
+ manual_control()
+ flag,print_count = keyboard_stop()
+
+ elif key == ord('v'):
+ flag = 3
+ while (flag == 3):
+ variables_change()
+ flag,print_count = keyboard_stop()
+
+ if print_count not 0:
+ print("No subsystem is running")
+ print_count = 0
--
GitLab