update main control

Code/Control/Laptop_Code/main_zhiying_move2goal_test.py

@@ -294,15 +294,15 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
     ballDetect  = ball_detect(gbx, gby)
     ballCapture = ball_capture(LIDAR_dist)
     goalDetect  = goal_detect(tx,ty)
+
     # debug
-    ballCapture = 0
     if ballCapture: # Ball captured
         if goalDetect:  # Goal detected
-            stop_all()
-            #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+            #stop_all()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
         else:  # Goal not detected
-            stop_all()
-            #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+            #stop_all()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
     else:  # Ball not captured
         if ballDetect:  # Ball detected
             pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
@@ -338,7 +338,7 @@ if __name__ == '__main__':
     gb_dist = -1         # by default (-1 means no found green ball)
 
     # Serial Port In
-    tx, ty, tz = 0, 0, 0  # by default (0 means no found AirTag)
+    tx, ty, tz = 100000, 100000, 100000  # by default (0 means no found AirTag)
     rx, ry, rz = 0, 0, 0
     LIDAR_dist = 0
     debugM = 'Testing'