Files
CPY_ScannerScript/scanner.py
T

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()