add keyboard manually determine green ball capture state function

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