diff --git a/Code/Control/Laptop_Code/main_integrated_test.py b/Code/Control/Laptop_Code/main_integrated_test.py index 4a24a9f2ae00fd46fc1589b4bace713aab74853a..1f1efaec3e203ba0e8f06debd4dda15db9cd2a30 100644 --- a/Code/Control/Laptop_Code/main_integrated_test.py +++ b/Code/Control/Laptop_Code/main_integrated_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()) @@ -328,8 +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(modelAction, device, trainLoc, labelSet, modelLoc, modelFile) + model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) # =========== DECLARE VARIABLES =========== @@ -348,31 +347,27 @@ if __name__ == '__main__': dir1, dir2, dir3, dir4 = '+', '+', '+', '+' # =========== LOOP FOREVER=========== - # ESP32_SLAVE Talk First - # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) + # Trigger the ESP32_SLAVE Talk First + 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) + time.sleep(waitTime) - - 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 ===== - + pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM) # ===== STEP 3: FEED ALL OUTPUT ===== - + serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) - time.sleep(0.05) + time.sleep(waitTime)