add keyboard manually determine green ball capture state function

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()