添加现有代码V1.6
This commit is contained in:
+501
@@ -0,0 +1,501 @@
|
||||
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)
|
||||
move_to_bottom(5000)
|
||||
loop_wait(200)
|
||||
move_to_origin()
|
||||
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()
|
||||
Reference in New Issue
Block a user