diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py index b38848bd07bdbda15da2956fab6a4e27886c8be0..98281b353b81ccb3af9a67585e3b20f1891aeaf3 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()