diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index d82613177590319aeebe23d10a0037a787f11bde..69101c8154a2c6388175c90c40aeb2a29811f974 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -7,14 +7,16 @@ import pygame
from constants import *
from ESP32_AT.imageTread_AT import get_AT_6DOF_info
-global ml
+global ml,esp_cam_on,openmv_on
ml = 0
+esp_cam_on = 0
+openmv_on = 1
if ml == 1:
import ball_detection.ball_detection as ball_detection
# ========= Serial Port I/O ===========
-def serial_port_in(serial_port):
+def serial_port_in_v1(serial_port):
'''
Description:
Take all ESP32_Master serial port's printIn and take all necessary input object
@@ -63,6 +65,33 @@ def serial_port_in(serial_port):
return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
+def serial_port_in_v2(serial_port):
+ '''
+ Description:
+ Take all ESP32_Master serial port's printIn and take all necessary input object
+
+ Input:
+ serial_port : serial.Serail object
+
+ Output:
+ LIDAR_dist,DebugM
+ '''
+ # debug info
+
+ print("initiating serial in V2...")
+ for i in range(2):
+ line = serial_port.readline()
+
+ if i == 0:
+ val = int(line.decode())
+ LIDAR_dist = val
+ elif i == 1:
+ debugM = line.decode()
+
+ print("Lidar_dist:{}".format(LIDAR_dist))
+ print(debugM)
+
+ return LIDAR_dist, debugM
def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
'''
@@ -123,7 +152,7 @@ def ball_capture(LIDAR_dist):
return False
def AT_detect(tx,ty):
- if tx == 0 and ty == 0:
+ if tx == 100000 and ty == 100000:
return False
else:
return True
@@ -193,7 +222,7 @@ def move2goal(tx, ty):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-def seeking():
+def ball_seeking(count_h,tx,ty):
"""
Description:
By default, when there ball is not determined capture, the manuver of the
@@ -206,31 +235,35 @@ def seeking():
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
-
- delta_h = 2
- url = 'http://192.168.1.118/cam-hi.jpg'
- tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ global set_h
+ if openmv_on == 1:
+ delta_h = 100
+ threshold_h = 20
+ elif esp_cam_on == 1:
+ delta_h = 2 # meter
+ threshold_h = 0.3
AT_detected = AT_detect(tx, ty)
- count_h = 0
- threshold_h = 0.3
+ # count_h = 0 #
if AT_detected:
current_h = get_altitude_from_AT(AT_h1,ty)
- count_h += 1
- if count_h == 1:
+
+ if count_h == 0:
set_h = current_h + delta_h # in meter
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+ count_h = 1 # moving up
+
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
diff_h = abs(set_h - current_h)
if diff_h < threshold_h:
new_ssp = 200
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+ count_h = 0
else:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
else:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
-
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def vertical_control(current_h,set_h):
@@ -328,7 +361,7 @@ def move2ball(gbx, gby, gb_dist):
# =========== Main Control ===========
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h):
'''
Description:
Given green ball information and AT information, the main control logic
@@ -367,23 +400,30 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
# stop_all() # Debug
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h):
# ===== STEP 1: TAKE ALL INPUT =====
# 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(waitTime)
+ if openmv_on == 1:
+ if line == b'SERIAL_IN_START\r\n':
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in_v1(serial_port)
+ print("gbx,gby:{},{}".format(gbx, gby))
+ time.sleep(waitTime)
+
+
+ if esp_cam_on == 1:
+ url = 'http://192.168.1.118/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
# ===== 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)
+ debugM,count_h)
# ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -396,7 +436,8 @@ def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist,
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
+ count_h = 0
+ return init_count,count_h
def init():
pygame.init()
@@ -545,7 +586,6 @@ def get_altitude_from_AT(AT_h,ty):
altitude = AT_h - ty
return altitude
-
def test_function():
url = 'http://192.168.1.118/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
@@ -605,8 +645,8 @@ if __name__ == '__main__':
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)
+ init_count,count_h = 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,count_h)
flag, print_count = keyboard_stop(flag,print_count)
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()