add test2

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', это название модуля с дальномерами

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()) + 

test.py

@@ -1,28 +1,35 @@
 from TelloModule import TelloModule
 import time
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+
 
 mod = TelloModule()
+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)
 
-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)
+plt.ioff()    
+plt.show()
 mod.module_deinit()
-print ("\n ------------ Finish Test ------------ \n")

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()