diff --git a/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py b/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py
index 3b922eb952023d69318047cd3a4713ab6e89dd95..4a24a9f2ae00fd46fc1589b4bace713aab74853a 100644
--- a/Code/Control/Laptop_Code/main_zhiying_move2goal_test.py
+++ b/Code/Control/Laptop_Code/main_zhiying_move2goal_test.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
@@ -23,7 +23,7 @@ def serial_port_in(serial_port):
# DEBUG Verbose
print("initiating one round of serial in ...")
-
+
for i in range(7):
line = serial_port.readline()
val = int(line.decode())
@@ -319,7 +319,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
+ PORT = "COM20" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -328,7 +328,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)
#model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
@@ -338,7 +338,7 @@ if __name__ == '__main__':
gb_dist = -1 # by default (-1 means no found green ball)
# Serial Port In
- tx, ty, tz = 100000, 100000, 100000 # by default (0 means no found AirTag)
+ tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
rx, ry, rz = 0, 0, 0
LIDAR_dist = 0
debugM = 'Testing'
@@ -349,23 +349,30 @@ if __name__ == '__main__':
# =========== LOOP FOREVER===========
# ESP32_SLAVE 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)
-
+
while True:
# ===== 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':
+
tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+
print("gbx,gby:{},{}".format(gbx,gby))
+ time.sleep(0.05)
- # ===== STEP 2: MAIN CONTROL LOOP =====
+
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)
+ # ===== STEP 2: MAIN CONTROL LOOP =====
+
# ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
- time.sleep(waitTime)
+ time.sleep(0.05)