def cantilever_height_adjustment(ser):
"""
悬臂高度调整 用红绿指示灯调整悬臂上下高度位置是否合适
:return:
"""
print('3333')
gl.set_value('picture', 5)
ser_top_plate = serial_open('/dev/lm') # 打开上顶板激光
Red_Led = 1 # 定义 1号管脚为红灯
Green_Led = 12 # 定义12号管脚为绿灯
distance_top_middle_plate = 102 # The distance between the top plate and the middle plate
GPIO.setup(Green_Led, GPIO.OUT) # 设置为输出管脚
GPIO.setup(Red_Led, GPIO.OUT)
# 设置红灯亮 绿灯灭
GPIO.output(Red_Led, GPIO.HIGH)
GPIO.output(Green_Led, GPIO.LOW)
time.sleep(3)
sendInstructions(ser, 'Sb500000!')
# sendInstructions(ser, 'SbA00000!') # 打开陀螺仪
# while True:
# if ser.read() == b'#':
#
# gyroscope_recv = gyroscope(ser)
# print(gyroscope_recv)
# else:
# gyroscope_recv = 361
#
# recv_top_plate = laser_receive(ser_top_plate) - distance_top_middle_plate
# print('recv_top_plate', recv_top_plate)
# if 250 < recv_top_plate < 265 and (0 <= gyroscope_recv <= 2 or 358 <= gyroscope_recv <= 360):
# sendInstructions(ser, 'SbB0000!') # 打开陀螺仪
# # 设置红灯灭 绿灯亮
# GPIO.output(Green_Led, GPIO.HIGH)
# GPIO.output(Red_Led, GPIO.HIGH)
# buzzer()
# break
while True:
#if ser.read() == b'#':
#gyroscope_recv = gyroscope(ser)
#else:
#gyroscope_recv = 361
recv_top_plate = laser_receive(
ser_top_plate) - distance_top_middle_plate
print('recv_top_plate', recv_top_plate)
if 260 < recv_top_plate < 270: #and (0 <= gyroscope_recv <= 2 or 358 <= gyroscope_recv <= 360):
# sendInstructions(ser, 'SbB00000!') # 陀螺仪
# 设置红灯灭 绿灯亮
buzzer()
GPIO.output(Green_Led, GPIO.HIGH)
GPIO.output(Red_Led, GPIO.LOW)
gl.set_value('picture', 7)
break
Flag = True
sendInstructions(ser, 'Sb200000!') # 语音6
return Flag
def dispatch(c=None, request=None):
ser = serial_open('/dev/ttyAMA0', 115200) # 打开串口与单片机通信 串口打开函数，波特率 115200
while True:
Flag_start = gl.get_value('Flag_start')
Flag_pause = gl.get_value('Flag_pause')
Flag_reset = gl.get_value('Flag_reset')
Flag_init = gl.get_value('Flag_init')
Flag_demo = gl.get_value('Flag_demo')
gl.set_value('picture', 4)
if Flag_demo == True:
print('demo')
demo_main_business()
gl.set_value('Flag_demo', False)
if Flag_start == True: # or request['mode'] == "MainStream":
thread_main = main_stream_thread(c, request)
thread_realtime = thread_realtime_show(c, request)
thread_main.start()
thread_realtime.start()
gl.set_value('Flag_start', False)
elif Flag_pause == True:
gl.set_value('Flag_pause', False)
while True:
if Flag_init == True:
gl.set_value('Flag_init', False)
break
elif Flag_reset == True:
ser.write(bytes('SZF00000!', 'utf-8'))
gl.set_value('Flag_start', False)
class thread_LCD(threading.Thread):
def __init__(self):
super().__init__()
def run(self): # 以下就可以编写线程需要的功能
LCD()
logger_config() # 配置日志文件
gl._init()
GPIO.setmode(GPIO.BCM)
button_t = thread_button()
button_t.start()
LCD_t = thread_LCD()
LCD_t.start()
gl.set_value('picture', 0)
dispatch()
#
# while True:
# try:
# gl._init() # 初始化各全局变量
# # 创建socket对象
# c = socket.socket()
# # 连接套接字（服务器）
# c.connect(('localhost', 11111))
# print("连接套接字（服务器）完成")
# break
# except BaseException as e:
# print("服务器还未开启，等待中。。。。。")
# time.sleep(2)
def main_business():
while True:
# 前期处理
ser = serial_open('/dev/ttyAMA0',
115200) # 打开串口与单片机通信 串口打开函数，波特率115200
GPIO.setwarnings(
False) # 如果RPi.GRIO检测到一个引脚已经被设置成了非默认值，那么你将看到一个警告信息 可以通过代码禁用警告
GPIO.setmode(GPIO.BCM) # 设置树莓派GPIO管脚模式为 BCM 模式
print('11111')
ser.write(bytes('ScA00000!', 'utf-8'))
# ser.write(bytes('SaA00007!', 'utf-8'))
sendInstructions(ser, 'SaA00007!')
time.sleep(0.5)
sendInstructions(ser, 'Sb800000!') #
Flag = cantilever_height_adjustment(ser) # 悬臂高度位置函数
# sendMessage(conection, request['id'], 1, "The height of the cantilever is suitable")
logging.info('The height of the cantilever is suitable')
time.sleep(5)
nipple_left, nipple_night, nipple_bone = two_cam_disparity() # 双目采集
print('22222')
if Flag:
sendInstructions(ser, 'Sb100000!')
print('33344')
sendInstructions(ser, 'SaA00000!')
logging.info('机械臂运动开始')
gl.set_value('picture', 8)
print('SaA00000!')
while True: # 等待机械臂初始化完成
recv = ser.read()
if recv == b'K':
break
# 数据收集
points_0, points_1 = get_points(ser) # data collection
# 数据处理
pointSet_e0, pointSet_m0, nipple_y_0, r_0_e = points_processing(
points_0, nipple_left, nipple_bone, mode=0)
pointSet_e1, pointSet_m1, nipple_y_1, r_1_e = points_processing(
points_1, nipple_night, nipple_bone, mode=1)
# 到达的坐标点
pointSet_eL, pointSet_mL, d0 = data_handle(pointSet_e0,
pointSet_m0,
nipple_y_0,
mode=0)
pointSet_eR, pointSet_mR, d1 = data_handle(pointSet_e1,
pointSet_m1,
nipple_y_1,
mode=1)
# 抬升的坐标点
pointSend_e0_t, pointSend_m0_t = normal_uplift(pointSet_eL,
pointSet_mL)
pointSend_e1_t, pointSend_m1_t = normal_uplift(pointSet_eR,
pointSet_mR)
probe_width = 50 # 普博设备的宽度
probe_depth = 10 # 普博设备的厚度
r = np.mean([r_0_e, r_1_e]) # 对两边半径进行取平均值
pointSet_edge_scan_l, edge_scan_num_l = edge_scan(
probe_width, probe_depth, -1 * r, pointSet_m0[0][5])
pointSet_edge_scan_r, edge_scan_num_r = edge_scan(
probe_width, probe_depth, r, pointSet_m0[0][5])
edge_scan_num = min(edge_scan_num_l, edge_scan_num_r)
print('r_0_e', r_0_e)
print('r_1_e', r_1_e)
logging.info(r_0_e)
logging.info(r_1_e)
# sendInstructions(ser, 'Sb90000!')
########## S-scan################
point_s_0, num = s_scan(probe_width, probe_depth, -1 * r,
pointSet_m0[0][5])
point_s_1, num = s_scan(probe_width, probe_depth, r, pointSet_m1[0][5])
# 运动算法处理
point_edge, Flag_edge, points_edge = control_algorithm(
pointSet_eL, pointSend_e0_t, pointSet_eR, pointSend_e1_t, d0, d1)
point_mid, Flag_mid, points_mid = control_algorithm(
pointSet_mL, pointSend_m0_t, pointSet_mR, pointSend_m1_t, d0, d1)
normal = [0, 0, 1]
point_side, Flag_side, points_side = Square_scanning(
normal, edge_scan_num, pointSet_edge_scan_l, pointSet_edge_scan_r,
d0, d1)
# point_s, Flag_s = scan_s(num, normal, point_s_0, point_s_1, d0, d1)
# communication(ser, point_s, Flag_s,conection,request)
# 数据传输
# 保存信息
save_file('points_edge.txt', points_edge, 1)
save_file('points_mid.txt', points_mid, 1)
save_file('points_side.txt', points_side, 1)
save_file('point_s_0.txt', point_s_0, 1)
save_file('point_s_1.txt', point_s_1, 1)
save_file('pointSet_eL.txt', pointSet_eL, 1)
save_file('pointSet_eR.txt', pointSet_eR, 1)
save_file('pointSet_mL.txt', pointSet_mL, 1)
save_file('pointSet_mR.txt', pointSet_mR, 1)
if Flag_edge == 0 and Flag_mid == 0 and Flag_side == 0:
ser.write(bytes('Sb00000!', 'utf-8'))
# sendInstructions(ser, 'Sb00000!')
communication(ser, point_edge, Flag_edge)
if Flag_mid == 0 and Flag_edge == 0 and Flag_edge == 0:
communication(ser, point_mid, Flag_mid)
communication(ser, point_side, Flag_side)
gl.set_value('picture', 11)
time.sleep(1)
logging.info('SYA00000!')
# ser.write(bytes('Sb700000!', 'utf-8'))
sendInstructions(ser, 'SaA00008!')
sendInstructions(ser, 'Sb700000!')
gl.set_value('picture', 12)
sendInstructions(ser, 'SYA00000!')
GPIO.output(1, GPIO.LOW)
GPIO.output(12, GPIO.LOW)
while True:
recv_y = ser.read()
print('recv_y', recv_y)
if recv_y == b'1':
print('-----------------------end---------------------')
break
else:
print('----------error----------------')
gl.set_value('picture', 12)
Mechanical_arm_reset(ser)
print('----------error----------------')
break