diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index d2efd7764c9904535574bf9c3b511226fd612d5d..a7fc2b9cfc4a8602e06d875df5254fc610c8abec 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -1,16 +1,13 @@
import time
import serial
-import pygame
+import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
-import cv2
-
+import timeit
+import pygame
from constants import *
global ml
-ml = 0
-if ml == 1:
- import ball_detection.ball_detection as ball_detection
-
+ml = 1
# ========= Serial Port I/O ===========
def serial_port_in(serial_port):
@@ -107,7 +104,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
@@ -288,7 +285,10 @@ 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
+
+ # debug
+ # ballCapture = 1
+ # goalDetect = 0
if ballCapture: # Ball captured
if goalDetect: # Goal detected
# stop_all() # Debug
@@ -305,33 +305,9 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-# ============= keyboard interruption ===================
-
-def init():
- pygame.init()
- win= pygame.display.set_mode((200,200))
-
-def keyboard_stop(flag_s,print_count_s):
-
- if get_key('q'):
- flag_s = 0
- print_count_s = 1
- return flag_s,print_count_s
-
-def get_key(keyname):
- ans = False
- for eve in pygame.event.get(): pass
- keyInput = pygame.key.get_pressed()
- myKey = getattr(pygame,'K_{}'.format(keyname))
-
- if keyInput[myKey]:
- ans = True
- pygame.display.update()
- return ans
-
def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
# ===== 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':
@@ -348,62 +324,40 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
time.sleep(waitTime)
-def decode_ctl(Ctl_com):
- pwm1 = Ctl_com[0]
- pwm2 = Ctl_com[1]
- pwm3 = Ctl_com[2]
- pwm4 = Ctl_com[3]
- dir1 = Ctl_com[4]
- dir2 = Ctl_com[5]
- dir3 = Ctl_com[6]
- dir4 = Ctl_com[7]
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-def manual_command(val1,val2,val3,val4,sign1,sign2,sign3,sign4):
- pwm1 = val1
- pwm2 = val2
- pwm3 = val3
- pwm4 = val4
- dir1 = sign1
- dir2 = sign2
- dir3 = sign3
- dir4 = sign4
- return pwm1,pwm2,pwm3,pwm4,dir1,dir2,dir3,dir4
-
-def manual_control(Ctl_com,serial_port):
-
- if get_key("w"):
- val = start_speed
- Ctl_com = manual_command(val, val, 0, 0, "+", "+", "+", "+")
-
- elif get_key("s"):
- val = start_speed
- Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
-
- if get_key("UP"):
- val = start_speed
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "+")
- elif get_key("DOWN"):
- val = start_speed
- Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "-")
-
- elif get_key("LEFT"):
- val = start_speed
- Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "+")
+ 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)
+ init_count += 1
+ return init_count
- elif get_key("RIGHT"):
- val = start_speed
- Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-")
+def init():
+ pygame.init()
+ win= pygame.display.set_mode((200,200))
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+def keyboard_stop(flag_s,print_count_s):
- waitTime = 0.05
+ if get_key('q'):
+ flag_s = 0
+ print_count_s = 1
+ return flag_s,print_count_s
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+def get_key(keyname):
+ ans = False
+ for eve in pygame.event.get(): pass
+ keyInput = pygame.key.get_pressed()
+ myKey = getattr(pygame,'K_{}'.format(keyname))
- time.sleep(waitTime)
+ if keyInput[myKey]:
+ ans = True
+ pygame.display.update()
+ return ans
+def stop_all():
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
def dynamic_variable(str_name_v):
@@ -458,11 +412,68 @@ def variables_change_once():
str_name = input("Enter your variable: ")
dynamic_variable(str_name)
+def manual_control(Ctl_com,serial_port):
+
+ if get_key("w"):
+ val = start_speed
+ Ctl_com = manual_command(val, val, 0, 0, "+", "+", "+", "+")
+
+ elif get_key("s"):
+ val = start_speed
+ Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
+
+ if get_key("UP"):
+ val = start_speed
+
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "+")
+ elif get_key("DOWN"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "-")
+
+ elif get_key("LEFT"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "+")
+
+ elif get_key("RIGHT"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-")
+
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+
+ waitTime = 0.05
+ # changed
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+def manual_command(val1,val2,val3,val4,sign1,sign2,sign3,sign4):
+ pwm1 = val1
+ pwm2 = val2
+ pwm3 = val3
+ pwm4 = val4
+ dir1 = sign1
+ dir2 = sign2
+ dir3 = sign3
+ dir4 = sign4
+ return pwm1,pwm2,pwm3,pwm4,dir1,dir2,dir3,dir4
+
+def decode_ctl(Ctl_com):
+ pwm1 = Ctl_com[0]
+ pwm2 = Ctl_com[1]
+ pwm3 = Ctl_com[2]
+ pwm4 = Ctl_com[3]
+ dir1 = Ctl_com[4]
+ dir2 = Ctl_com[5]
+ dir3 = Ctl_com[6]
+ dir4 = Ctl_com[7]
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM3" # Based on your own serial port number
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -471,12 +482,10 @@ if __name__ == '__main__':
waitTime = 0.05
# Loading the PyTorch ML model for ball detection
- if ml == 1:
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES ===========
# ESP CAM In
- global gbx,gby,gb_dist
gbx, gby = -1, -1 # by default (-1 means no found green ball)
gb_dist = -1 # by default (-1 means no found green ball)
@@ -489,29 +498,29 @@ if __name__ == '__main__':
# Serial Port Out
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
# Trigger the ESP32_SLAVE to talk first
- if ml == 1:
- 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)
-
- Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+ """
flag = 0
- print_count = 1
+ init_count = 0
+ print_count = 0
global start_speed
start_speed = 70
init()
# =========== LOOP FOREVER===========
while True:
-
if get_key('a'):
flag = 1
while (flag == 1):
+ if init_count == 0:
+ init_count = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
flag, print_count = keyboard_stop(flag,print_count)
-
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -533,6 +542,6 @@ if __name__ == '__main__':
elif get_key('k'):
break
- if print_count is not 0:
+ if print_count != 0:
print("No subsystem is running")
print_count = 0