From bb70deacf83dd4b3b849472e703bf1511c4e7184 Mon Sep 17 00:00:00 2001 From: Zen Date: Tue, 4 Aug 2020 19:45:28 +0300 Subject: [PATCH] add test2 --- README.md | 8 ++++++-- TelloModule.py | 38 +++++++++++++++++++++++++--------- test.py | 55 ++++++++++++++++++++++++++++---------------------- test2.py | 19 +++++++++++++++++ 4 files changed, 85 insertions(+), 35 deletions(-) create mode 100644 test2.py diff --git a/README.md b/README.md index f39dda4..86765f9 100644 --- a/README.md +++ b/README.md @@ -92,7 +92,9 @@ mod = TelloModule() 3. Далее нужно понимать что есть 2 режима работы библиотеки это Блокирующий и не Блокирующий. -В блокирующим режиме при вызове метода приема информации с модуля, ждем когда придут новые значения, и только после этого скрипт продолжит исполнятся. Для его работы будет достаточно вызвать метод чтения значения и ждать его завершения. +### Блокирующий режим работы + +В данном режиме при вызове метода приема информации с модуля, ждем когда придут новые значения, и только после этого скрипт продолжит исполнятся. Для его работы будет достаточно вызвать метод чтения значения и ждать его завершения. ```python line = mod.get_block_dist() @@ -109,7 +111,9 @@ line = mod.get_block_dist() Так-же тут необходимо отметить что все команды для коптера работают в блокирующем режиме. -В не блокирующим режиме при вызове одного из методов происходит чтение прошлого значения с модуля из-за чего не происходит блокировки. Для использования этого режима нужна инициализация и де инициализация. +### НЕ Блокирующий режим работы + +В этом режиме при вызове одного из методов происходит чтение прошлого значения с модуля из-за чего не происходит блокировки. Для использования этого режима нужна инициализация и де инициализация. ```python mod.module_init('distance') # инициализация модуля 'distance', это название модуля с дальномерами diff --git a/TelloModule.py b/TelloModule.py index dfd5e88..468d717 100644 --- a/TelloModule.py +++ b/TelloModule.py @@ -1,6 +1,7 @@ import serial import time import threading +import serial.tools.list_ports info = { 'count':0, 'forward_dist':0, @@ -29,20 +30,39 @@ class SerialTread (threading.Thread): self.c = 1 class TelloModule(): - def __init__(self): - """Constructor""" - pass - - def module_init(self, module): + + def __init__(self, module = 'distance'): + global serial_port global type_module + + serial_port = "" + type_module = module + + ports = serial.tools.list_ports.comports() + for p in ports: + if p.manufacturer == 'SBRICKS': + serial_port = p.device + if serial_port == "": + print ("Error: SMART BRICKS receiver not found, please insert it into your computer's USB port.") + exit() + + line = None + ser = serial.Serial(serial_port, 115200, timeout=2) + line = ser.readline() + ser.close() + if line == b'': + print ("Error: module " + module + " not found, please enable it.") + exit() + + + def module_init(self): global t global ser t = SerialTread("Thread", 1) - ser = serial.Serial('/dev/ttyUSB0', 115200) + ser = serial.Serial(serial_port, 115200) ser.readline() - type_module = module if type_module == 'distance': t.start() time.sleep(1) @@ -82,7 +102,7 @@ class TelloModule(): return [info['forward_dist'], info['right_dist'], info['left_dist'], info['back_dist']] def get_block_dist(self): # count + forward + right + back + left + light - ser = serial.Serial('/dev/ttyUSB0', 115200) + ser = serial.Serial(serial_port, 115200) line = str(ser.readline())[4:-3].split() ser.close() if line[0][3] == "r" : @@ -106,7 +126,7 @@ if __name__ == "__main__": " light = " + str(line[5])) print ("\n ------ Test NON Blocking mode ------- \n") - mod.module_init('distance') + mod.module_init() for i in range(10): print ("Test " + str(i) + " count = " + str(mod.get_count()) + diff --git a/test.py b/test.py index 8cac1a3..9a86898 100644 --- a/test.py +++ b/test.py @@ -1,28 +1,35 @@ from TelloModule import TelloModule import time +import matplotlib.pyplot as plt +import matplotlib.animation as animation + mod = TelloModule() - -print ("\n -------- Test blocking mode --------- \n") -for i in range(10): - line = mod.get_block_dist() - print ("Test " + str(i) + - " count = " + str(line[0]) + - " forward = " + str(line[1]) + - " right = " + str(line[2]) + - " back = " + str(line[3]) + - " left = " + str(line[4]) + - " light = " + str(line[5])) -print ("\n ------ Test NON Blocking mode ------- \n") -mod.module_init('distance') -for i in range(10): - print ("Test " + str(i) + - " count = " + str(mod.get_count()) + - " forward = " + str(mod.get_forward_dist()) + - " right = " + str(mod.get_right_dist()) + - " back = " + str(mod.get_back_dist()) + - " left = " + str(mod.get_left_dist()) + - " light = " + str(mod.get_light())) - time.sleep(0.5) -mod.module_deinit() -print ("\n ------------ Finish Test ------------ \n") \ No newline at end of file +fig = plt.figure() +plt.ion() + +x = 100 +y = 150 +line1 = plt.plot([x, 0],[0, y], c = 'b') +line2 = plt.plot([-x, 0],[0, y], c = 'b') +line3 = plt.plot([-x, 0],[0, -y], c = 'b') +line4 = plt.plot([x, 0],[0, -y], c = 'b') +line5 = plt.plot([-x/3, x/3], [y-y/3, y-y/3], c = 'b') + +mod.module_init() + +point = [None, None, None, None] +while 1: + if (point[0] != None): + for p in point: + p.remove() + point[0] = plt.scatter(0, mod.get_forward_dist(), c = 'r') + point[1] = plt.scatter(mod.get_right_dist(), 0, c = 'r') + point[2] = plt.scatter(-mod.get_left_dist(), 0, c = 'r') + point[3] = plt.scatter(0, -mod.get_back_dist(), c = 'r') + + plt.pause(0.1) + +plt.ioff() +plt.show() +mod.module_deinit() \ No newline at end of file diff --git a/test2.py b/test2.py new file mode 100644 index 0000000..f0507aa --- /dev/null +++ b/test2.py @@ -0,0 +1,19 @@ +from easytello import tello +from TelloModule import TelloModule + +drone = tello.Tello() +mod = TelloModule() + +def main(): + drone.takeoff() + while 1: + line = mod.get_block_dist() + if int(line[1]) > 400: + drone.forward(20) + if int(line[1]) < 300: + drone.back(20) + +try: + main() +except KeyboardInterrupt: + drone.land() \ No newline at end of file