fix the bug

Code/Control/Laptop_Code/main_zzl.py

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