503 lines
14 KiB
Python
503 lines
14 KiB
Python
import gc
|
|
import json
|
|
import binascii
|
|
|
|
from daq import *
|
|
from motor import *
|
|
from json_config import read_config, save_config
|
|
from net import httpc_post_data, udp_send, udp_recv, httpc_post_data_json_bin
|
|
from webserver import ws_send_status, ws_send_data
|
|
|
|
# 定义电机状态枚举类
|
|
class ScannerState:
|
|
STOP = 'stop'
|
|
MOVING_UP = 'moving_up'
|
|
MOVING_DOWN = 'moving_down'
|
|
STOP_AT_UPPER = 'stop_at_upper'
|
|
STOP_AT_BOTTOM = 'stop_at_bottom'
|
|
STOP_AT_ORIGIN = 'stop_at_origin'
|
|
|
|
motor_ready = False
|
|
daq_temp = 0
|
|
daq_online = 0
|
|
daq_version = ''
|
|
|
|
motor_state = ScannerState.STOP
|
|
motor_speed = 0
|
|
motor_pos = 0
|
|
|
|
bottom_pos = 0
|
|
upper_pos = 0
|
|
|
|
reset_flag = False
|
|
run_flag = False
|
|
|
|
scan_count = 0xFFFFFFFF
|
|
scan_poeriod = 6000
|
|
|
|
lw_upper = pyb.Pin(pyb.Pin.cpu.B14, pyb.Pin.IN)
|
|
lw_lower = pyb.Pin(pyb.Pin.cpu.B15, pyb.Pin.IN)
|
|
|
|
xray_pin = pyb.Pin(pyb.Pin.cpu.C0, pyb.Pin.OUT_PP)
|
|
xray_pin.value(0)
|
|
|
|
wdt_pin = pyb.Pin(pyb.Pin.cpu.E4, pyb.Pin.OUT_PP)
|
|
wdt_pin.value(0)
|
|
wdt_pin_val = False
|
|
|
|
try:
|
|
config = read_config()
|
|
scan_period = config['scan_period']
|
|
except:
|
|
pass
|
|
|
|
def watch_dog_feed():
|
|
global wdt_pin_val
|
|
if wdt_pin_val:
|
|
wdt_pin_val = False
|
|
wdt_pin.value(0)
|
|
else:
|
|
wdt_pin_val = True
|
|
wdt_pin.value(1)
|
|
|
|
def update_status():
|
|
status = {
|
|
'id': config['code'],
|
|
'run_flag' : run_flag,
|
|
'run_time' : time.ticks_ms(),
|
|
'state': str(motor_state), # 转换为字符串发送
|
|
'speed': motor_speed,
|
|
'pos': motor_pos,
|
|
'upper_pos': upper_pos,
|
|
'bottom_pos': bottom_pos,
|
|
'scan_range': upper_pos - bottom_pos,
|
|
'daq_temp' : daq_temp,
|
|
'daq_version' : daq_version,
|
|
'daq_online' : daq_online > 0
|
|
}
|
|
packet = json.dumps(status)
|
|
if not udp_send(packet.encode('utf-8')):
|
|
print('发送状态失败')
|
|
ws_send_status('/dev/status/scanner', packet)
|
|
# print('发送状态成功', packet)
|
|
|
|
def move_to(dir,timeout):
|
|
global motor_state, motor_speed, motor_pos
|
|
if dir == 'up':
|
|
if lw_upper.value() == 0:
|
|
return False
|
|
motor_state = ScannerState.MOVING_UP
|
|
motor_speed_set(40)
|
|
elif dir == 'down':
|
|
if lw_lower.value() == 0:
|
|
return False
|
|
motor_state = ScannerState.MOVING_DOWN
|
|
motor_speed_set(-40)
|
|
else:
|
|
print(f"未知方向{dir}")
|
|
return False
|
|
start_time = time.ticks_ms()
|
|
update_time = start_time
|
|
update_status()
|
|
while True:
|
|
pos, speed = motor_poll()
|
|
if not (speed is None):
|
|
motor_speed = speed
|
|
if not (pos is None):
|
|
motor_pos = pos
|
|
time.sleep_ms(5)
|
|
if time.ticks_diff(time.ticks_ms(), update_time) > 300:
|
|
update_status()
|
|
update_time = time.ticks_ms()
|
|
if dir == 'up' and lw_upper.value() == 0:
|
|
motor_speed_set(0)
|
|
motor_state = ScannerState.STOP_AT_UPPER
|
|
update_status()
|
|
return True
|
|
if dir == 'down' and lw_lower.value() == 0:
|
|
motor_speed_set(0)
|
|
motor_state = ScannerState.STOP_AT_BOTTOM
|
|
update_status()
|
|
return True
|
|
if time.ticks_diff(time.ticks_ms(), start_time) > timeout:
|
|
print(f"移动到{dir}超时")
|
|
motor_speed_set(0)
|
|
motor_state = ScannerState.STOP
|
|
update_status()
|
|
return False
|
|
udp_data_check()
|
|
watch_dog_feed()
|
|
|
|
|
|
def move_to_top(timeout):
|
|
global upper_pos
|
|
if move_to('up', timeout):
|
|
loop_wait(500)
|
|
upper_pos = motor_pos
|
|
print("bottom pos update", motor_pos)
|
|
|
|
def move_to_bottom(timeout):
|
|
global bottom_pos
|
|
if move_to('down', timeout):
|
|
loop_wait(500)
|
|
bottom_pos = motor_pos
|
|
print("bottom pos update", motor_pos)
|
|
|
|
def loop_wait(timeout):
|
|
global motor_speed, motor_pos
|
|
start_time = time.ticks_ms()
|
|
update_time = start_time
|
|
while True:
|
|
pos, speed = motor_poll()
|
|
if not (speed is None):
|
|
motor_speed = speed
|
|
if not (pos is None):
|
|
motor_pos = pos
|
|
if time.ticks_diff(time.ticks_ms(), start_time) > timeout:
|
|
break
|
|
time.sleep_ms(10)
|
|
if time.ticks_diff(time.ticks_ms(), update_time) > 200:
|
|
update_status()
|
|
update_time = time.ticks_ms()
|
|
watch_dog_feed()
|
|
udp_data_check()
|
|
|
|
def move_to_pos(pos, timeout):
|
|
motor_pos_set(pos)
|
|
tim = time.ticks_ms()
|
|
while True:
|
|
loop_wait(100)
|
|
watch_dog_feed()
|
|
if abs(motor_pos - pos) < 0.2:
|
|
break
|
|
if time.ticks_diff(time.ticks_ms(), tim) > timeout:
|
|
break
|
|
loop_wait(300)
|
|
motor_set_mode_speed()
|
|
loop_wait(300)
|
|
if abs(motor_pos - pos) < 0.2:
|
|
return True
|
|
return False
|
|
|
|
def read_daq():
|
|
data_len = daq_get_sample_len()
|
|
if data_len is None or data_len == 0:
|
|
print("读取数据长度失败或者没有数据")
|
|
return None
|
|
print(f"数据长度: {data_len}")
|
|
recv_data = daq_get_sample_data()
|
|
if recv_data is None:
|
|
print("读取数据失败")
|
|
else:
|
|
l = len(recv_data)
|
|
print(f"接收数据: {l},移动距离: {l * 0.04 / 4}mm")
|
|
return recv_data
|
|
|
|
def make_data_packet(direction, data):
|
|
content = {
|
|
'direction': direction,
|
|
'id': config['code'],
|
|
'data': binascii.b2a_base64(data).decode('utf-8')
|
|
}
|
|
return content
|
|
|
|
def read_and_send_data_json():
|
|
gc.collect()
|
|
|
|
recv_data = read_daq()
|
|
if recv_data is None:
|
|
return
|
|
if motor_state == ScannerState.STOP_AT_UPPER:
|
|
direction = 'UP'
|
|
elif motor_state == ScannerState.STOP_AT_BOTTOM:
|
|
direction = 'DOWN'
|
|
else:
|
|
print('电机位置异常,取消数据发送')
|
|
return
|
|
packet = make_data_packet(direction, recv_data)
|
|
recv_data = None
|
|
# gc.collect()
|
|
|
|
packet = json.dumps(packet)
|
|
# print(gc.mem_free(), len(packet))
|
|
|
|
gc.collect()
|
|
|
|
if httpc_post_data(packet):
|
|
print('发送数据成功')
|
|
else:
|
|
print('发送数据失败')
|
|
print()
|
|
|
|
def read_and_send_data():
|
|
gc.collect()
|
|
|
|
recv_data = read_daq()
|
|
if recv_data is None:
|
|
return
|
|
if motor_state == ScannerState.STOP_AT_UPPER:
|
|
direction = 'UP'
|
|
elif motor_state == ScannerState.STOP_AT_BOTTOM:
|
|
direction = 'DOWN'
|
|
else:
|
|
print('电机位置异常,取消数据发送')
|
|
return
|
|
|
|
tim = time.ticks_ms()
|
|
if httpc_post_data_json_bin({'direction': direction, 'id': config['code'], 'data_len': len(recv_data)}, recv_data):
|
|
print('http发送数据成功')
|
|
else:
|
|
print('http发送数据失败')
|
|
print(f"http发送数据耗时: {time.ticks_diff(time.ticks_ms(), tim)}ms")
|
|
gc.collect()
|
|
|
|
if ws_send_data('/dev/data/scanner', recv_data):
|
|
print('ws发送数据成功')
|
|
else:
|
|
print('ws发送数据失败')
|
|
|
|
print(gc.mem_free())
|
|
gc.collect()
|
|
|
|
class CommandException(Exception):
|
|
pass
|
|
|
|
def udp_data_check():
|
|
global reset_flag, run_flag, scan_count, scan_period
|
|
|
|
__udp_cmd = udp_recv()
|
|
if not __udp_cmd:
|
|
return
|
|
if not motor_ready:
|
|
return
|
|
try:
|
|
cmd_obj = json.loads(__udp_cmd.decode('utf-8'))
|
|
cmd = cmd_obj['cmd']
|
|
except:
|
|
print('命令格式错误')
|
|
return
|
|
|
|
if cmd == 'reset':
|
|
run_flag = False
|
|
reset_flag = True
|
|
print('reset cmd')
|
|
raise CommandException()
|
|
elif cmd == 'start':
|
|
try:
|
|
if cmd_obj['count'] > 0:
|
|
scan_count = cmd_obj['count']
|
|
if cmd_obj['period'] >= 4000 and cmd_obj['period'] <= 60 * 1000:
|
|
scan_period = cmd_obj['period']
|
|
if config['scan_period'] != scan_period:
|
|
config.update('scan_period', scan_period)
|
|
save_config(config)
|
|
except:
|
|
pass
|
|
if scan_count <= 0:
|
|
scan_count = 0xFFFFFFFF
|
|
if run_flag and not reset_flag:
|
|
if motor_state == ScannerState.STOP_AT_BOTTOM \
|
|
or motor_state == ScannerState.STOP_AT_UPPER:
|
|
print('start rightnow')
|
|
raise CommandException()
|
|
return
|
|
|
|
if not run_flag:
|
|
reset_flag = False
|
|
run_flag = True
|
|
print('start cmd')
|
|
raise CommandException()
|
|
elif cmd == 'emer_stop':
|
|
if not run_flag:
|
|
return
|
|
run_flag = False
|
|
print('stop cmd')
|
|
raise CommandException()
|
|
elif cmd == 'stop':
|
|
run_flag = False
|
|
print('stop cmd')
|
|
else:
|
|
print(f"未知命令{cmd}")
|
|
return
|
|
|
|
def prepare_before_scanning():
|
|
daq_set_relay(True)
|
|
xray_pin.value(1)
|
|
loop_wait(1000)
|
|
daq_set_sample(True)
|
|
|
|
def sweep_after_scaning():
|
|
daq_set_sample(False)
|
|
xray_pin.value(0)
|
|
loop_wait(200)
|
|
daq_set_relay(False)
|
|
|
|
def scan():
|
|
if motor_state == ScannerState.STOP_AT_BOTTOM:
|
|
print('scan up')
|
|
prepare_before_scanning()
|
|
move_to_top(5000)
|
|
sweep_after_scaning()
|
|
read_and_send_data()
|
|
|
|
elif motor_state == ScannerState.STOP_AT_UPPER:
|
|
print('scan down')
|
|
prepare_before_scanning()
|
|
move_to_bottom(5000)
|
|
sweep_after_scaning()
|
|
read_and_send_data()
|
|
|
|
def daq_sample_with_time(tim = 2500):
|
|
prepare_before_scanning()
|
|
loop_wait(tim)
|
|
sweep_after_scaning()
|
|
|
|
gc.collect()
|
|
recv_data = read_daq()
|
|
if recv_data is None:
|
|
print("data empty")
|
|
return
|
|
packet = make_data_packet("UP", recv_data)
|
|
recv_data = None
|
|
gc.collect()
|
|
packet = json.dumps(packet)
|
|
print(gc.mem_free())
|
|
|
|
if httpc_post_data(packet):
|
|
print('发送数据成功')
|
|
else:
|
|
print('发送数据失败')
|
|
print()
|
|
|
|
def scanner_loop():
|
|
global motor_ready, motor_state, run_flag, reset_flag, scan_count, scan_period, daq_online, daq_temp, daq_version
|
|
motor_init()
|
|
daq_init()
|
|
|
|
watch_dog_feed()
|
|
|
|
daq_set_relay(False)
|
|
|
|
daq_version = daq_read_version()
|
|
if daq_version is None:
|
|
print("读取版本失败")
|
|
|
|
param = daq_read_param()
|
|
print(param)
|
|
if param and param.get('ucUseEncoder', -1) != 1:
|
|
daq_set_default_param()
|
|
param = None
|
|
|
|
loop_wait(1000)
|
|
|
|
if lw_lower.value() != 0:
|
|
move_to_bottom(5000)
|
|
loop_wait(500)
|
|
|
|
move_to_top(5000)
|
|
loop_wait(500)
|
|
|
|
move_to_bottom(5000)
|
|
loop_wait(500)
|
|
|
|
move_to_origin()
|
|
motor_state = ScannerState.STOP_AT_ORIGIN
|
|
|
|
print('back to origin')
|
|
print("bottom pos", bottom_pos)
|
|
print("upper pos", upper_pos)
|
|
|
|
motor_ready = True
|
|
|
|
while True:
|
|
try:
|
|
if run_flag:
|
|
gc.collect()
|
|
tim = time.ticks_ms()
|
|
scan()
|
|
if scan_count > 0:
|
|
scan_count = scan_count - 1
|
|
if scan_count <= 0:
|
|
run_flag = False
|
|
if run_flag:
|
|
delta = scan_poeriod - time.ticks_diff(time.ticks_ms(), tim)
|
|
if delta < 1000:
|
|
wait_time = 1000
|
|
else:
|
|
wait_time = delta
|
|
|
|
loop_wait(wait_time)
|
|
else:
|
|
temp = daq_check_is_online()
|
|
if temp is not None:
|
|
daq_temp = temp
|
|
daq_online = 2
|
|
else:
|
|
if daq_online > 0:
|
|
daq_online -= 1
|
|
loop_wait(1000)
|
|
gc.collect()
|
|
# print(gc.mem_free())
|
|
# pyb.info()
|
|
# print("gc collect")
|
|
# gc.collect()
|
|
|
|
except KeyboardInterrupt:
|
|
motor_speed_set(0)
|
|
xray_pin.value(0)
|
|
daq_set_relay(False)
|
|
daq_set_sample(False)
|
|
print("测试结束")
|
|
run_flag = False
|
|
reset_flag = False
|
|
|
|
break
|
|
except CommandException:
|
|
print("cmd exception")
|
|
if reset_flag:
|
|
xray_pin.value(0)
|
|
daq_set_relay(False)
|
|
daq_set_sample(False)
|
|
motor_speed_set(0)
|
|
loop_wait(200)
|
|
daq_set_power(False)
|
|
move_to_bottom(5000)
|
|
loop_wait(200)
|
|
move_to_origin()
|
|
daq_set_power(True)
|
|
motor_state = ScannerState.STOP_AT_ORIGIN
|
|
print('back to origin')
|
|
reset_flag = False
|
|
update_status()
|
|
else:
|
|
if run_flag:
|
|
print(motor_state)
|
|
|
|
if motor_state == ScannerState.STOP_AT_ORIGIN:
|
|
if bottom_pos > 1:
|
|
print("move to bottom")
|
|
if move_to_pos(bottom_pos, 5000):
|
|
motor_state = ScannerState.STOP_AT_BOTTOM
|
|
loop_wait(300)
|
|
print("pos", motor_pos)
|
|
elif motor_pos < 1:
|
|
motor_state = ScannerState.STOP_AT_BOTTOM
|
|
elif motor_state == ScannerState.STOP:
|
|
if motor_pos > bottom_pos + 1:
|
|
move_to_bottom(5000)
|
|
if move_to_pos(bottom_pos, 5000):
|
|
motor_state = ScannerState.STOP_AT_BOTTOM
|
|
loop_wait(300)
|
|
else:
|
|
xray_pin.value(0)
|
|
daq_set_relay(False)
|
|
daq_set_sample(False)
|
|
motor_speed_set(0)
|
|
print('stop')
|
|
motor_state = ScannerState.STOP
|
|
update_status()
|
|
|
|
|
|
# 运行测试
|
|
if __name__ == "__main__":
|
|
scanner_loop() |