diff --git a/Code/Control/Laptop_Code/main_zzl.py b/Code/Control/Laptop_Code/main_zzl.py index 951b0a07297e35839435eb8dc65da28540bdacb5..3c0ba85d36341bc48ac6697958b9d0170fc3fd05 100644 --- a/Code/Control/Laptop_Code/main_zzl.py +++ b/Code/Control/Laptop_Code/main_zzl.py @@ -1,6 +1,6 @@ import time import serial -# import ball_detection.ball_detection as ball_detection +import ball_detection.ball_detection as ball_detection import simple_pid.PID as PID import timeit @@ -102,7 +102,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 @@ -285,8 +285,8 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): goalDetect = goal_detect(tx, ty) # debug - ballCapture = 1 - goalDetect = 0 + # ballCapture = 1 + # goalDetect = 0 if ballCapture: # Ball captured if goalDetect: # Goal detected # stop_all() # Debug @@ -305,7 +305,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): # ===== STEP 1: TAKE ALL INPUT ===== - # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) + gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) line = serial_port.readline() if line == b'SERIAL_IN_START\r\n': @@ -322,11 +322,19 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di time.sleep(waitTime) +def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): + + 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" # for Alienware + PORT = "COM6" # for Alienware serial_port = serial.Serial(PORT, 115200) serial_port.close() serial_port.open() @@ -335,7 +343,7 @@ if __name__ == '__main__': waitTime = 0.05 # Loading the PyTorch ML model for ball detection - # model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) + model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) # =========== DECLARE VARIABLES =========== # ESP CAM In @@ -353,11 +361,14 @@ if __name__ == '__main__': dir1, dir2, dir3, dir4 = '+', '+', '+', '+' # Trigger the ESP32_SLAVE to talk first - # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) + """ + 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 = 0 # =========== LOOP FOREVER=========== while True: + 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) -