upload keyboard logic to main code

Code/Control/Laptop_Code/main_keyboard.py

@@ -301,21 +301,28 @@ 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'):
-        stop_all()
-    elif key == ord('c'):
-        manual_control()
-    elif key == ord('v'):
-        variables_change()
+
+def init():
+    pygame.init()
+    win= pygame.display.set_mode((200,200))
+
+def keyboard_stop(flag_s,print_count_s):
+
+    if get_key('q'):
+        flag_s = 0
+        print_count_s = 1
+    return flag_s,print_count_s
+
+def get_key(keyname):
+    ans = False
+    for eve in pygame.event.get(): pass
+    keyInput = pygame.key.get_pressed()
+    myKey = getattr(pygame,'K_{}'.format(keyname))
+
+    if keyInput[myKey]:
+        ans = True
+    pygame.display.update()
+    return ans
 
 def auto_control():
     # =================================== tested autonomous control ======================================================
@@ -382,32 +389,36 @@ if __name__ == '__main__':
 
     flag = 0
     print_count = 1
+    init()
     # =========== LOOP FOREVER===========
     while True:
 
-        key = cv2.waitKey(5)
-
-        if key == ord('a'):
+        if get_key('a'):
             flag = 1
             while (flag == 1):
                 auto_control()
-                flag,print_count = keyboard_stop()
+                flag, print_count = keyboard_stop(flag,print_count)
+                if flag == 0:
+                    cap.release()
+                    cv2.destroyAllWindows()
 
-        elif key == ord('s'):
+        elif get_key('s'):
             stop_all()
             print("stop all motors")
 
-        elif key == ord('c'):
+        elif get_key('m'):
             flag = 2
             while (flag == 2):
                 manual_control()
-                flag,print_count = keyboard_stop()
+                flag, print_count = keyboard_stop(flag,print_count)
 
-        elif key == ord('v'):
+        elif get_key('v'):
             flag = 3
             while (flag == 3):
                 variables_change()
-                flag,print_count = keyboard_stop()
+                flag, print_count = keyboard_stop(flag,print_count)
+        elif get_key('k'):
+            break
 
         if print_count is not 0:
             print("No subsystem is running")