diff --git a/Code/Control/Laptop_Code/main_integrated_test.py b/Code/Control/Laptop_Code/main_integrated_test.py index 41dad51d6a4eb16cef6c6e9336366e9c0256ecd9..6fa51abe6cfd199810c3d39fc23c796cfd1a4724 100644 --- a/Code/Control/Laptop_Code/main_integrated_test.py +++ b/Code/Control/Laptop_Code/main_integrated_test.py @@ -1,11 +1,14 @@ import time import serial -import ball_detection.ball_detection as ball_detection + import simple_pid.PID as PID import timeit from constants import * - +global ml +ml = 1 +if ml ==1: + import ball_detection.ball_detection as ball_detection # ========= Serial Port I/O =========== def serial_port_in(serial_port): @@ -102,7 +105,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 @@ -167,15 +170,21 @@ def move2goal(tx, ty): # Horizontal lspeed = -1 * outputx + base_speed rspeed = 1 * outputx + base_speed - pwm3 = abs(lspeed) - pwm4 = abs(rspeed) + # pwm3 = abs(lspeed) + # pwm4 = abs(rspeed) + pwm3 = min(abs(lspeed), 255) + pwm4 = min(abs(rspeed), 255) if (lspeed > 0): dir3 = '+' + # dir3 = '-' else: dir3 = '-' + # dir3 = '+' if (rspeed > 0): + #dir4 = '+' dir4 = '+' else: + # dir4 = '-' dir4 = '-' return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4 @@ -283,7 +292,7 @@ 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) - + ballCapture = 1 if ballCapture: # Ball captured if goalDetect: # Goal detected # stop_all() # Debug @@ -314,7 +323,8 @@ if __name__ == '__main__': waitTime = 0.05 # Loading the PyTorch ML model for ball detection - model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) + if ml == 1: + model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) # =========== DECLARE VARIABLES =========== # ESP CAM In @@ -332,14 +342,16 @@ 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) + if ml == 1: + 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) # =========== LOOP FOREVER=========== while True: # ===== STEP 1: TAKE ALL INPUT ===== - gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) + if ml == 1: + gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) line = serial_port.readline() if line == b'SERIAL_IN_START\r\n': @@ -353,4 +365,4 @@ if __name__ == '__main__': # ===== STEP 3: FEED ALL OUTPUT ===== serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) - time.sleep(waitTime) \ No newline at end of file + time.sleep(waitTime)