You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
tello_modules/TelloModule.py

143 lines
4.2 KiB

4 years ago
import serial
import time
import threading
4 years ago
import serial.tools.list_ports
4 years ago
info = { 'count':0,
'forward_dist':0,
'right_dist':0,
'back_dist':0,
'left_dist':0,
'light':0 }
class SerialTread (threading.Thread):
c = 0
def __init__(self, name, counter):
threading.Thread.__init__(self)
self.threadID = counter
self.name = name
self.counter = counter
def run(self):
while self.c == 0:
line = str(ser.readline())[4:-3].split()
info['count'] = int(line[0][2:])
info['forward_dist'] = int(line[1][6:])
info['right_dist'] = int(line[2][6:])
info['back_dist'] = int(line[3][6:])
info['left_dist'] = int(line[4][6:])
info['light'] = int(line[5][6:])
def join(self):
self.c = 1
class TelloModule():
4 years ago
def __init__(self, module = 'distance'):
global serial_port
4 years ago
global type_module
4 years ago
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):
4 years ago
global t
global ser
t = SerialTread("Thread", 1)
4 years ago
ser = serial.Serial(serial_port, 115200)
4 years ago
ser.readline()
if type_module == 'distance':
t.start()
time.sleep(1)
else:
print ("Error: There is NO module with this name, please select \"distance\".")
def module_deinit(self):
if type_module == 'distance':
t.join()
type_module == ''
else:
print ("Error: First you need to initialize the module.")
def get_forward_dist(self):
return info['forward_dist']
def get_left_dist(self):
return info['left_dist']
def get_right_dist(self):
return info['right_dist']
def get_back_dist(self):
return info['back_dist']
def get_light(self):
return info['light']
def get_count(self):
return info['count']
def get_dist(self):
return [info['forward_dist'], info['right_dist'], info['left_dist'], info['back_dist']]
def get_block_dist(self): # count + forward + right + back + left + light
4 years ago
ser = serial.Serial(serial_port, 115200)
4 years ago
line = str(ser.readline())[4:-3].split()
ser.close()
if line[0][3] == "r" :
return [int(line[0][6:]), int(line[1][6:]), int(line[2][6:]), int(line[3][6:]), int(line[4][6:]), int(line[5][6:])]
else:
return [int(line[0]), int(line[1][6:]), int(line[2][6:]), int(line[3][6:]), int(line[4][6:]), int(line[5][6:])]
if __name__ == "__main__":
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")
4 years ago
mod.module_init()
4 years ago
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")