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

123 lines
3.6 KiB

import serial
import time
import threading
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():
def __init__(self):
"""Constructor"""
pass
def module_init(self, module):
global type_module
global t
global ser
t = SerialTread("Thread", 1)
ser = serial.Serial('/dev/ttyUSB0', 115200)
ser.readline()
type_module = module
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
ser = serial.Serial('/dev/ttyUSB0', 115200)
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")
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")