Skip to content
Snippets Groups Projects
Commit 663140e2 authored by Zhiying Li's avatar Zhiying Li
Browse files

change the move2goal function based on the field test

parent 3f8c158c
Branches
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Code/Control/Laptop_Code/main_keyboard.py
+ 85
− 31
View file @ 663140e2
......@@ -8,9 +8,11 @@ from constants import *
from ESP32_AT.imageTread_AT import get_AT_6DOF_info
global ml,esp_cam_on,openmv_on
ml = 1
esp_cam_on = 0
openmv_on = 1
ml = 0
esp_cam_on = 1
openmv_on = 0
seekVertDir = 1
AT_detected_time = time.time()
if ml == 1:
import ball_detection.ball_detection as ball_detection
......@@ -31,7 +33,7 @@ def serial_port_in_v1(serial_port):
# DEBUG Verbose
print("initiating one round of serial in ...")
for i in range(8):
for i in range(7):
line = serial_port.readline()
val = int(line.decode())
......@@ -49,9 +51,10 @@ def serial_port_in_v1(serial_port):
rz = val
elif i == 6:
LIDAR_dist1 = val
elif i == 7:
LIDAR_dist2 = val
# elif i == 7:
# LIDAR_dist2 = val
LIDAR_dist2 = 0
line = serial_port.readline()
debugM = line.decode()
......@@ -190,16 +193,17 @@ def move2goal(tx, ty,tz):
setpoint_x1 = 0.0
setpoint_y1 = 0.0
if tz <200:
kpy_g,kiy_g,kdy_g = 0.0, 0.0, 0.0
base_speed = 150
AT_goal_Delta = 150
if tz < 2.0:
kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
base_speed = 200
AT_goal_Delta = -150
else:
kpy_g,kiy_g,kdy_g = 2, 0.01, 0.5
base_speed = 120
kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
# kpy_g,kiy_g,kdy_g = 0.0, 0.00, 0.0
base_speed = 140
AT_goal_Delta = 0
inputx = tx / 1.00
inputy = (ty + AT_goal_Delta) / 1.00 #
inputx = tx * 100/ 1.00
inputy = (ty * 100 + AT_goal_Delta) / 1.00 #
pid_x = PID(kpx_g, kix_g, kdx_g, setpoint = setpoint_x1)
pid_y = PID(kpy_g, kiy_g, kdy_g, setpoint = setpoint_y1)
......@@ -211,17 +215,17 @@ def move2goal(tx, ty,tz):
outputx = pid_x(inputx)
outputy = pid_y(inputy)
print("outputy:{}".format(outputy))
# Vertical
pwm1 = abs(outputy)
pwm2 = abs(outputy)
if(outputy > 0):
dir1 = '-'
dir2 = '-'
else:
dir1 = '+'
dir2 = '+'
else:
dir1 = '-'
dir2 = '-'
# Horizontal
lspeed = -1 * outputx + base_speed
......@@ -253,9 +257,9 @@ def ball_seeking(count_h,tx,ty):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
global set_h
global set_h, seekVertDir
if openmv_on == 1:
delta_h = 100
delta_h = 100 # cm
threshold_h = 20
elif esp_cam_on == 1:
delta_h = 2 # meter
......@@ -282,6 +286,23 @@ def ball_seeking(count_h,tx,ty):
else:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
# if time.time() > AT_detected_time + AT_detectBreak:
# AT_detected = AT_detect(tx, ty)
# AT_detected_time = time.time()
#
# while facingWall():
# pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
#
# if AT_detected:
# current_h = get_altitude_from_AT(AT_h1,ty)
#
# if LIDAR_dist2 < LIDAR2_Thres:
# seekVertDir = 0
# if baroHeight < baroThres:
# seekVertDir = 1
# pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move_in_spiral(new_ssp, seekVertDir)
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def vertical_control(current_h,set_h):
......@@ -317,6 +338,14 @@ def rotate_one_direction_ssp(ssp):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def move_in_spiral(ssp, dir):
pwm1, pwm2, pwm3, pwm4 = ssp, ssp, ssp, ssp/2
dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
if dir == 0:
dir1, dir2 = '-', '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def move2ball(gbx, gby, gb_dist):
"""
Description:
......@@ -394,6 +423,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
print('in main_control')
# placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
......@@ -402,12 +432,14 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
ballCapture = ball_capture(LIDAR_dist1)
goalDetect = goal_detect(tx, ty)
ballCapture = 1
ballCapture = 0
# debug
# ballCapture = 1
ballCapture = 1
# goalDetect = 0
# ballDetect = 0
if ballCapture: # Ball captured
print('ballCapture TRUE')
if goalDetect: # Goal detected
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty,tz)
......@@ -415,21 +447,24 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
else: # Ball not captured
print('ballCapture FALSE')
if ballDetect: # Ball detected
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 = ball_seeking(count_h,tx,ty)
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_dist1, LIDAR_dist2, debugM,count_h):
# ===== STEP 1: TAKE ALL INPUT =====
# print('in auto_control')
if ml ==1:
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
line = serial_port.readline()
# print('auto')
if openmv_on == 1:
if line == b'SERIAL_IN_START\r\n':
tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM = serial_port_in_v1(serial_port)
......@@ -438,8 +473,12 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
if esp_cam_on == 1:
url = 'http://192.168.1.118/cam-hi.jpg'
url = 'http://192.168.0.204/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
LIDAR_dist1 = 0
LIDAR_dist2 = 0
debugM = "using two esp32 cam"
# ===== 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_dist1, LIDAR_dist2,
......@@ -452,7 +491,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM):
count_h = 0
# gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
# gbx, gby, gb_dist = ball_detection.detectLive(url_gb, 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_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
init_count += 1
......@@ -572,7 +611,7 @@ def manual_control(Ctl_com,serial_port):
waitTime = 0.05
# changed
# gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
# gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
......@@ -606,8 +645,21 @@ 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'
url = 'http://192.168.0.230/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
print("testing new function")
print("-----------------------")
print("tid:{}".format(tid))
print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
def test_function1():
url = 'http://192.168.0.230/cam-hi.jpg'
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
print("testing new function")
print("-----------------------")
......@@ -616,6 +668,7 @@ def test_function():
print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
......@@ -651,7 +704,7 @@ if __name__ == '__main__':
Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
# Trigger the ESP32_SLAVE to talk first
"""
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
gbx, gby, gb_dist = ball_detection.detectLive(url_gb, 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_dist1, LIDAR_dist2, debugM, count_h)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
"""
......@@ -669,6 +722,7 @@ if __name__ == '__main__':
while (flag == 1):
if init_count == 0:
init_count,count_h = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
print('auto_control')
auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h)
flag, print_count = keyboard_stop(flag,print_count)
elif get_key('s'):
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment