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.
123 lines
3.6 KiB
123 lines
3.6 KiB
4 years ago
|
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")
|
||
|
|
|
||
|
|
|