diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/previous_high_level/main_keyboard.py similarity index 96% rename from Code/Control/Laptop_Code/main_keyboard.py rename to Code/Control/Laptop_Code/previous_high_level/main_keyboard.py index 757c83833bbe7a9ae71737881cd0067760f6f9f6..ce27fa6b8eae25e1b900a181115529b711b5b544 100644 --- a/Code/Control/Laptop_Code/main_keyboard.py +++ b/Code/Control/Laptop_Code/previous_high_level/main_keyboard.py @@ -7,7 +7,7 @@ import cv2 from constants import * global ml -ml = 0 +ml = 1 if ml == 1: import ball_detection.ball_detection as ball_detection @@ -107,7 +107,7 @@ def goal_detect(tx,ty): ''' return True if April Tag is detected ''' - if tx == 100000 and ty == 100000: + if tx == 0 and ty == 0: return False else: return True @@ -399,6 +399,8 @@ def manual_control(Ctl_com,serial_port): pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com) waitTime = 0.05 + # changed + gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) @@ -458,11 +460,18 @@ def variables_change_once(): str_name = input("Enter your variable: ") dynamic_variable(str_name) +def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): + if ml == 1: + gbx, gby, gb_dist = ball_detection.detectLive(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_dist, debugM) + serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) + init_count += 1 + return init_count # ===== Main Function ===== if __name__ == '__main__': # =========== SET UP ============ # Defining Variables for ESP 32 Serial I/O - PORT = "COM22" # Based on your own serial port number + PORT = "COM6" # Based on your own serial port number serial_port = serial.Serial(PORT, 115200) serial_port.close() serial_port.open() @@ -491,14 +500,12 @@ if __name__ == '__main__': dir1, dir2, dir3, dir4 = '+', '+', '+', '+' # Trigger the ESP32_SLAVE to talk first - if ml == 1: - gbx, gby, gb_dist = ball_detection.detectLive(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_dist, debugM) - serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) + Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"] flag = 0 print_count = 1 + init_count = 0 global start_speed start_speed = 70 @@ -509,6 +516,9 @@ if __name__ == '__main__': if get_key('a'): flag = 1 while (flag == 1): + if init_count == 0: + init_count = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM) + auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM) flag, print_count = keyboard_stop(flag,print_count)