diff --git a/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py b/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py index c2c46537f0b99a5457d976f95cd6d90ee58bff63..3b922eb952023d69318047cd3a4713ab6e89dd95 100644 --- a/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py +++ b/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'