1010 lines
46 KiB
Python
1010 lines
46 KiB
Python
import os, time, csv, struct, sys
|
||
import numpy as np
|
||
sys.path.append(r'.\UserScripts')
|
||
from user_common import wordData2HexStr, nowStr, nowStr1, checkValue, params_to_int
|
||
from CRC import crc16 as checkValue
|
||
|
||
SENDHEADER = ''
|
||
RSPHEADER = ''
|
||
RETRYTIMES = 3
|
||
CYCLES = 1
|
||
TIMEOUT = 0.05
|
||
DEBUG = False
|
||
|
||
# 启动函数，命令开始会调用此函数
|
||
def start():
|
||
global recvData, deviceData
|
||
try:
|
||
log_i(f"[{devInfo['name']}] {cmdInfo['name']}".center(80, '-'))
|
||
|
||
deviceData = _G.get(f"{devInfo['name']}")
|
||
if not deviceData:
|
||
deviceData = {}
|
||
if 'data' not in deviceData:
|
||
deviceData['data'] = {}
|
||
if 'info' not in deviceData:
|
||
deviceData['info'] = {}
|
||
if 'status' not in deviceData:
|
||
deviceData['status'] = {}
|
||
if 'config' not in deviceData:
|
||
raise Exception(f"Please load config first.")
|
||
|
||
deviceData['status']['StopAll'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
|
||
except Exception as e:
|
||
log_e(f"Error in start. {str(e)}")
|
||
finish()
|
||
|
||
# 此函数会被重复调用，间隔10毫秒，直到finish()
|
||
def loop():
|
||
global recvData, deviceData
|
||
try:
|
||
LRSOV_Open()
|
||
LRSOV_Close()
|
||
LRSOV_Open()
|
||
LRSOV_Close()
|
||
finish()
|
||
except Exception as e:
|
||
log_e(f"Error in loop. {str(e)}")
|
||
finish()
|
||
|
||
# 接收数据处理函数，当收到数据会调用此函数
|
||
def recvDataHandler(data):
|
||
global recvData
|
||
try:
|
||
recvData = recvData + data
|
||
except Exception as e:
|
||
log_e(f"Error in recvDataHandler. {str(e)}")
|
||
finish()
|
||
|
||
def log_du(str):
|
||
if DEBUG:
|
||
log_d(str)
|
||
|
||
def exeCmd(cmd):
|
||
try:
|
||
global recvData
|
||
data = bytearray().fromhex(cmd[0])
|
||
data += bytearray(checkValue(data).to_bytes(2, 'little'))
|
||
for i in range(RETRYTIMES):
|
||
log_du(f"[{nowStr()}] Sent:{wordData2HexStr(data)}")
|
||
recvData = bytearray()
|
||
send(data)
|
||
time.sleep(TIMEOUT)
|
||
rspLen = int(cmd[1])
|
||
if len(recvData) >= rspLen:
|
||
log_du(f"[{nowStr()}] Echo:{wordData2HexStr(recvData[0:rspLen])}")
|
||
crc = int.from_bytes(recvData[rspLen-2:rspLen], byteorder='little')
|
||
calc_value = checkValue(recvData[0:rspLen-2])
|
||
# log(f"{crc:04X}, {calc_value:04X}")
|
||
if crc == calc_value:
|
||
return [True, recvData[0:rspLen]]
|
||
return [False, None]
|
||
except Exception as e:
|
||
raise Exception(f"Error in exeCmd({cmd}): {str(e)}")
|
||
|
||
def LRSOV_Enable():
|
||
try:
|
||
log(f"LRSOV_Enable starts.")
|
||
ret = exeCmd(['0306 0080 0001', 8])
|
||
if ret[0]:
|
||
log(f"LRSOV_Enable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"LRSOV_Enable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in LRSOV_Enable(): str{e}")
|
||
return False
|
||
|
||
def LRSOV_Disable():
|
||
try:
|
||
log(f"LRSOV_Disable starts.")
|
||
ret = exeCmd(['0306 0080 0000', 8])
|
||
if ret[0]:
|
||
log(f"LRSOV_Disable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"LRSOV_Disable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in LRSOV_Disable(): str{e}")
|
||
return False
|
||
|
||
def LRSOV_Open():
|
||
try:
|
||
log(f"LRSOV_Open starts.")
|
||
cmdList = [
|
||
['0306 0080 0001', 8], #0，使能电机
|
||
['0303 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0306 0084 0001', 8], #2，预设打开操作
|
||
['0303 00A4 0004', 13], #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0303 008C 0001', 7] #4，查询电机速度，当速度为0时，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos <= deviceData['config']['LRSOV']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设打开操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 3
|
||
case 3: #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos <= deviceData['config']['LRSOV']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
case 4: #4，查询电机速度，当速度为0时，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curSpeed, = struct.unpack('>h', ret[1][3:5])
|
||
if curSpeed == 0:
|
||
Info = f"Speed is equal to 0."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 3
|
||
case _: #5，禁用电机，退出函数
|
||
ret = exeCmd(['0306 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"LRSOV_Open stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in LRSOV_Open(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def LRSOV_Close():
|
||
global deviceData
|
||
try:
|
||
log(f"LRSOV_Close starts.")
|
||
cmdList = [
|
||
['0306 0080 0001', 8], #0，使能电机
|
||
['0303 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0306 0084 0000', 8], #2，预设关闭操作
|
||
['0303 00A4 0004', 13], #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0303 008C 0001', 7] #4，查询电机速度，当速度为0时，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos >= deviceData['config']['LRSOV']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设关闭操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =3
|
||
case 3: #3，查询位置信息，当位置在0点附近，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos >= deviceData['config']['LRSOV']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 4
|
||
case 4: #4，查询电机速度，当速度为0时，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curSpeed, = struct.unpack('>h', ret[1][3:5])
|
||
if curSpeed == 0:
|
||
Info = f"Speed is equal to 0."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 3
|
||
case _: #5，禁用电机，退出函数
|
||
ret = exeCmd(['0306 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"LRSOV_Close stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in LRSOV_Close(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def PPSOV_Enable():
|
||
try:
|
||
log(f"PPSOV_Enable starts.")
|
||
ret = exeCmd(['0206 0080 0001', 8])
|
||
if ret[0]:
|
||
log(f"PPSOV_Enable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"PPSOV_Enable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in PPSOV_Enable(): str{e}")
|
||
return False
|
||
|
||
def PPSOV_Disable():
|
||
try:
|
||
log(f"PPSOV_Disable starts.")
|
||
ret = exeCmd(['0206 0080 0000', 8])
|
||
if ret[0]:
|
||
log(f"PPSOV_Disable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"PPSOV_Disable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in PPSOV_Disable(): str{e}")
|
||
return False
|
||
|
||
def PPSOV_Open():
|
||
try:
|
||
log(f"PPSOV_Open starts.")
|
||
cmdList = [
|
||
['0206 0080 0001', 8], #0，使能电机
|
||
['0203 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0206 0084 0001', 8], #2，预设打开操作
|
||
['0203 00A4 0004', 13], #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0203 008C 0001', 7] #4，查询电机速度，当速度为0时，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos <= deviceData['config']['PPSOV']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设打开操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 3
|
||
case 3: #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos <= deviceData['config']['PPSOV']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 4
|
||
case 4: #4，查询电机速度，当速度为0时，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curSpeed, = struct.unpack('>h', ret[1][3:5])
|
||
if curSpeed == 0:
|
||
Info = f"Speed is equal to 0."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 3
|
||
case _: #5，禁用电机，退出函数
|
||
ret = exeCmd(['0306 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"PPSOV_Open stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in PPSOV_Open(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def PPSOV_Close():
|
||
try:
|
||
log(f"PPSOV_Close starts.")
|
||
cmdList = [
|
||
['0206 0080 0001', 8], #0，使能电机
|
||
['0203 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0206 0084 0000', 8], #2，预设关闭操作
|
||
['0203 00A4 0004', 13], #3，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0203 008C 0001', 7] #4，查询电机速度，当速度为0时，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos >= deviceData['config']['PPSOV']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设关闭操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =3
|
||
case 3: #3，查询位置信息，当位置在0点附近，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos >= deviceData['config']['PPSOV']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 4
|
||
case 4: #4，查询电机速度，当速度为0时，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curSpeed, = struct.unpack('>h', ret[1][3:5])
|
||
if curSpeed == 0:
|
||
Info = f"Speed is equal to 0."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 3
|
||
case _: #5，禁用电机，退出函数
|
||
ret = exeCmd(['0306 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"PPSOV_Close stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in PPSOV_Close(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def PPECC_Enable():
|
||
try:
|
||
log(f"PPECC_Enable starts.")
|
||
ret = exeCmd(['0106 0080 0001', 8])
|
||
if ret[0]:
|
||
log(f"PPECC_Enable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"PPECC_Enable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in PPECC_Enable(): str{e}")
|
||
return False
|
||
|
||
def PPECC_Disable():
|
||
try:
|
||
log(f"PPECC_Disable starts.")
|
||
ret = exeCmd(['0106 0080 0000', 8])
|
||
if ret[0]:
|
||
log(f"PPECC_Disable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"PPECC_Disable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in PPECC_Disable(): str{e}")
|
||
return False
|
||
|
||
def PPECC_ReturnToZero():
|
||
try:
|
||
log(f"PPECC_ReturnToZero starts.")
|
||
cmdList = [
|
||
['0106 0080 0001', 8], #0，使能电机
|
||
['0103 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0106 0084 0000', 8], #2，预设回零操作
|
||
['0106 0081 0000', 8], #3，发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
['0106 0081 0001', 8], #4
|
||
['0103 00A4 0004', 13], #5，查询位置信息，当位置在0点附近，禁用电机
|
||
['0103 008C 0001', 7] #6，查询电机速度，当速度为0时，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if abs(curPos - deviceData['config']['PPECC']['zeroPosition'])<10 or curPos < deviceData['config']['PPECC']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
optStatus = 2
|
||
case 2: #2，预设回零操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =3
|
||
case 3: #3，发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =4
|
||
case 4: #4
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =5
|
||
case 5: #5，查询位置信息，当位置在0点附近，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if abs(curPos - deviceData['config']['PPECC']['zeroPosition'])<10 or curPos < deviceData['config']['PPECC']['zeroPosition']:
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 6
|
||
case 6: #6，查询电机速度，当速度为0时，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curSpeed, = struct.unpack('>h', ret[1][3:5])
|
||
if curSpeed == 0:
|
||
Info = f"Speed is equal to 0."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 5
|
||
case _: #-1，禁用电机，退出函数
|
||
ret = exeCmd(['0106 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"PPECC_ReturnToZero stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in PPECC_ReturnToZero(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def PPECC_Push(targetPressure):
|
||
try:
|
||
log(f"PPECC_Push starts.")
|
||
cmdList = [
|
||
['0106 0080 0001', 8], #0，使能电机
|
||
['0103 00A4 0004', 13], #1，查询位置信息，当位置在最大位置附近，退出指令执行
|
||
['0106 0084 0001', 8], #2，预设加压操作
|
||
['0106 0081 0000', 8], #3，发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
['0106 0081 0001', 8], #4
|
||
['0103 00A4 0004', 13], #5，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0103 0093 0001', 7], #6，查询当前转矩信息，当转矩超过阈值，退出指令执行
|
||
['0106 001A ', 8] #7，设置回零速度
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
curTorque = None
|
||
Info = ''
|
||
deviceData = _G.get(devInfo['name'])
|
||
if targetPressure:
|
||
targetPressure = int(targetPressure)
|
||
if 'pressure' not in deviceData['data'] or deviceData['data']['pressure']['unit'] != 'psi':
|
||
raise Exception(f"Can't get 'pressure' information or pressure unit isn't 'psi'.")
|
||
if int(deviceData['data']['pressure']['value']) > targetPressure:
|
||
Info = f"Current pressure is {deviceData['data']['pressure']['value']}psi."
|
||
optStatus = -1
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在最大位置附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if abs(curPos - deviceData['config']['PPECC']['maxPosition'])<10 or curPos > deviceData['config']['PPECC']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设加压操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 3
|
||
case 3: #3，发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 4
|
||
case 4: #4
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 5
|
||
case 5: #5，查询位置信息，当位置在最大位置附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if abs(curPos - deviceData['config']['PPECC']['maxPosition'])<10 or curPos > deviceData['config']['PPECC']['maxPosition']:
|
||
Info = f"It reached the max position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 6
|
||
case 6: #6，查询当前转矩信息，当转矩超过阈值，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curTorque, = struct.unpack('>H', ret[1][3:5])
|
||
if curTorque >= deviceData['config']['PPECC']['maxTorque']:
|
||
Info = f"It reached the max torque."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 7
|
||
case 7: #7, 判断是否到达压力
|
||
if (deviceData['data']['pressure']['value'] > 0.85*targetPressure):
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
curSpeed = abs(int((deviceData['data']['pressure']['value']-targetPressure))//10)
|
||
if curSpeed > 350 or deviceData['data']['pressure']['value']<0:
|
||
curSpeed = 350
|
||
elif curSpeed < 100:
|
||
curSpeed = 100
|
||
# if targetPressure>1000:
|
||
# if (deviceData['data']['pressure']['value'] > 0.85*targetPressure):
|
||
# optStatus = -1
|
||
# continue
|
||
# else:
|
||
# curSpeed = abs(int((deviceData['data']['pressure']['value']-targetPressure))//10)
|
||
# if curSpeed > 300:
|
||
# curSpeed = 300
|
||
# elif targetPressure > 200:
|
||
# if (deviceData['data']['pressure']['value'] > 0.85*targetPressure):
|
||
# optStatus = -1
|
||
# continue
|
||
# else:
|
||
# curSpeed = 200
|
||
# elif targetPressure>100:
|
||
# if (deviceData['data']['pressure']['value'] > 0.9*targetPressure):
|
||
# optStatus = -1
|
||
# continue
|
||
# else:
|
||
# curSpeed = 200
|
||
# else:
|
||
# if (deviceData['data']['pressure']['value'] > targetPressure-3):
|
||
# optStatus = -1
|
||
# continue
|
||
# else:
|
||
# curSpeed = 200
|
||
exeCmd([f"0106 001A {curSpeed:04X}", 8])
|
||
optStatus = 5
|
||
case _: #-1，禁用电机，退出函数
|
||
ret = exeCmd(['0106 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, curTorque={curTorque}, Info={Info}")
|
||
log(f"PPECC_Push stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in PPECC_Push(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, curTorque={curTorque}, Info={Info}. {str(e)}")
|
||
return False
|
||
|
||
def BTV_Enable():
|
||
try:
|
||
log(f"BTV_Enable starts.")
|
||
ret = exeCmd(['0406 0080 0001', 8])
|
||
if ret[0]:
|
||
log(f"BTV_Enable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"BTV_Enable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_Enable(): str{e}")
|
||
return False
|
||
|
||
def BTV_Disable():
|
||
try:
|
||
log(f"BTV_Disable starts.")
|
||
ret = exeCmd(['0406 0080 0000', 8])
|
||
if ret[0]:
|
||
log(f"BTV_Disable succeeded.")
|
||
return True
|
||
else:
|
||
log_e(f"BTV_Disable failed.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_Disable(): str{e}")
|
||
return False
|
||
|
||
def BTV_IsAtZero():
|
||
try:
|
||
log(f"BTV_IsAtZero starts.")
|
||
cmdList = [
|
||
['0403 00A4 0004', 13, 5] #查询位置信息，当位置在0点附近，退出指令执行
|
||
]
|
||
optStatus = 0
|
||
retryTimes = cmdList[optStatus][2]
|
||
while(retryTimes):
|
||
retryTimes -= 1
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if (curPos - deviceData['config']['BTV']['zeroPosition'])<= 10:
|
||
log(f"BTV_IsAtZero stopped. It reached zero position.")
|
||
return True
|
||
log_w(f"No response.")
|
||
log(f"BTV_IsAtZero stopped.")
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_IsAtZero(): str{e}")
|
||
return False
|
||
|
||
def BTV_ReturnToZero():
|
||
try:
|
||
log(f"BTV_ReturnToZero starts.")
|
||
cmdList = [
|
||
['0406 0080 0001', 8], #0，使能电机
|
||
['0403 00A4 0004', 13], #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
['0406 0084 0001', 8], #2，预设回零操作
|
||
['0403 00A4 0004', 13], #3，查询位置信息，当位置在0点附近，禁用电机
|
||
['0406 0080 0000', 8] #4，禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
curSpeed = None
|
||
Info = ''
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0: #0，使能电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1: #1，查询位置信息，当位置在0点附近，退出指令执行
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if curPos <= int(deviceData['config']['BTV']['zeroPosition']):
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
optStatus = 2
|
||
case 2: #2，预设回零操作
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus =3
|
||
case 3: #3，查询位置信息，当位置在0点附近，禁用电机
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if (curPos <= int(deviceData['config']['BTV']['zeroPosition'])):
|
||
Info = f"It reached the zero position."
|
||
optStatus = -1
|
||
continue
|
||
case _: #4，禁用电机，退出函数
|
||
ret = exeCmd(['0406 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"BTV_ReturnToZero stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_ReturnToZero(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def BTV_ControlPressure_bk(targetPressure, keepTime, conrtrolMode):
|
||
try:
|
||
#controlMode取0，当到达限位，禁用电机；controlMode取1，当到达限位，执行回零、加压操作；
|
||
global deviceData
|
||
log(f"BTV_ControlPressure starts.")
|
||
zeroPosition = int(deviceData['config']['BTV']['zeroPosition'])
|
||
maxPosition = int(deviceData['config']['BTV']['maxPosition'])
|
||
maxSpeed = int(deviceData['config']['BTV']['maxSpeed'])
|
||
minSpeed = int(deviceData['config']['BTV']['minSpeed'])
|
||
curSpeed = maxSpeed//2
|
||
curPressure = int(deviceData['data']['pressure']['value'])
|
||
curDirection = 1 # 1:正向，-1:反向
|
||
limitedError = int(deviceData['config']['BTV']['limitedError'])
|
||
cmdList = [
|
||
['0406 0080 0001', 8], #0 使能电机
|
||
[f"0410 0061 0002 04 {maxPosition:08X}", 8], #1 设定电机目标位置
|
||
['0406 0084 0000', 8], #2 预设加压操作
|
||
['0406 0081 0000', 8], #3 发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
['0406 0081 0001', 8], #4
|
||
['0406 0063', 8], #5 根据当前压力和目标压力的差值，调整电机速度
|
||
['0403 00A4 0004', 13], #6 查询位置信息，当位置到达边界位置，退出指令执行
|
||
['0406 0063 0000', 8], #7 设定电机目标速度为0, 停止电机
|
||
['0406 0080 0000', 8] #8 禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
startTime = time.time()
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
exeFlag =False
|
||
if not exeFlag:
|
||
log(f"BTV_ControlPressure stopped.")
|
||
return False
|
||
match optStatus:
|
||
case 0:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 2
|
||
case 2:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 3
|
||
case 3:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 4
|
||
case 4:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 5
|
||
case 5:
|
||
exeCmd(cmdList[0])
|
||
exeCmd(cmdList[3])
|
||
exeCmd(cmdList[4])
|
||
curPressure = int(deviceData['data']['pressure']['value'])
|
||
if abs(curPressure-targetPressure) > limitedError:
|
||
startTime = time.time()
|
||
if time.time()-startTime > keepTime:
|
||
log(f"BTV_ControlPressure stopped.")
|
||
return True
|
||
else:
|
||
if targetPressure >= curPressure:
|
||
curDirection = 1
|
||
else:
|
||
curDirection = -1
|
||
if abs(curPressure-targetPressure)>1000:
|
||
curSpeed = maxSpeed
|
||
else:
|
||
curSpeed = int(abs(curPressure-targetPressure)/1000*maxSpeed)
|
||
if abs(curSpeed) < abs(minSpeed):
|
||
curSpeed = minSpeed
|
||
curSpeed = curDirection*curSpeed
|
||
cmdList[optStatus][0] = (cmdList[optStatus][0][:9] + wordData2HexStr(struct.pack('>h', curSpeed)))
|
||
ret = exeCmd( cmdList[optStatus])
|
||
optStatus = 6
|
||
case 6:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if (curPos >= maxPosition+1000 and curSpeed >0) or (curPos <= zeroPosition-1000 and curSpeed <0):
|
||
log_w(f"Exceed position limit: curPos={curPos}")
|
||
if conrtrolMode == 1:
|
||
#执行回零加压操作
|
||
BTV_ReturnToZero()
|
||
if targetPressure-2000>0:
|
||
PPECC_Push(targetPressure-2000)
|
||
else:
|
||
PPECC_Push(0)
|
||
optStatus = 0
|
||
else:
|
||
#禁用电机
|
||
optStatus = 8
|
||
else:
|
||
optStatus = 5
|
||
case 7:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 5
|
||
case 8:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
log(f"BTV_ControlPressure stopped.")
|
||
return True
|
||
return False
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_ControlPressure(): str{e}")
|
||
return False
|
||
|
||
def BTV_ControlPressure(targetPressure, keepTime, conrtrolMode):
|
||
try:
|
||
#controlMode取0，当到达限位，禁用电机；controlMode取1，当到达限位，执行回零、加压操作；
|
||
log(f"BTV_ControlPressure starts.")
|
||
deviceData = _G.get(devInfo['name'])
|
||
deviceData['status']['flagPressureStable'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
zeroPosition = int(deviceData['config']['BTV']['zeroPosition'])
|
||
maxPosition = int(deviceData['config']['BTV']['maxPosition'])
|
||
maxSpeed = int(deviceData['config']['BTV']['maxSpeed'])
|
||
minSpeed = int(deviceData['config']['BTV']['minSpeed'])
|
||
curSpeed = maxSpeed//2
|
||
curPressure = int(deviceData['data']['pressure']['value'])
|
||
curDirection = 1 # 1:正向，-1:反向
|
||
limitedError = int(deviceData['config']['BTV']['limitedError'])
|
||
cmdList = [
|
||
['0406 0080 0001', 8], #0 使能电机
|
||
[f"0410 0061 0002 04 {maxPosition:08X}", 8], #1 设定电机目标位置
|
||
['0406 0084 0000', 8], #2 预设加压操作
|
||
['0406 0081 0000', 8], #3 发送2条写寄存器0x81指令，产生上升沿，启动预设操作
|
||
['0406 0081 0001', 8], #4
|
||
['0406 0063', 8], #5 根据当前压力和目标压力的差值，调整电机速度
|
||
['0403 00A4 0004', 13], #6 查询位置信息，当位置到达边界位置，退出指令执行
|
||
['0406 0063 0000', 8], #7 设定电机目标速度为0, 停止电机
|
||
['0406 0080 0000', 8] #8 禁用电机
|
||
]
|
||
optStatus = 0
|
||
exeFlag = True
|
||
curPos = None
|
||
Info = ''
|
||
startTime = time.time()
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 1
|
||
case 1:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 2
|
||
case 2:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 3
|
||
case 3:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 4
|
||
case 4:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 5
|
||
case 5:
|
||
exeCmd(cmdList[0])
|
||
curPressure = deviceData['data']['pressure']['value']
|
||
if abs(curPressure-targetPressure) > limitedError:
|
||
startTime = time.time()
|
||
deviceData['status']['flagPressureStable'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
if time.time()-startTime > 10:
|
||
deviceData['status']['flagPressureStable'] = True
|
||
_G.set(devInfo['name'], deviceData)
|
||
if time.time()-startTime > keepTime + 10:
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
deviceData['status']['flagPressureStable'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
if targetPressure >= curPressure:
|
||
curDirection = 1
|
||
else:
|
||
curDirection = -1
|
||
if abs(curPressure-targetPressure) > 1000:
|
||
curSpeed = maxSpeed
|
||
elif abs(curPressure-targetPressure) > limitedError/2:
|
||
curSpeed = int(abs(curPressure-targetPressure)/1000*maxSpeed)
|
||
if abs(curSpeed) < abs(minSpeed):
|
||
curSpeed = minSpeed
|
||
else:
|
||
curSpeed = 0
|
||
curSpeed = curDirection*curSpeed
|
||
cmdList[optStatus][0] = (cmdList[optStatus][0][:9] + wordData2HexStr(struct.pack('>h', curSpeed)))
|
||
ret = exeCmd(cmdList[optStatus])
|
||
optStatus = 6
|
||
case 6:
|
||
ret = exeCmd(cmdList[optStatus])
|
||
if ret[0]:
|
||
curPos, = struct.unpack('>i', ret[1][7:11])
|
||
if (curPos >= maxPosition+1000 and curSpeed >0) or (curPos <= zeroPosition-1000 and curSpeed <0):
|
||
log_w(f"Exceed position limit: curPos={curPos}")
|
||
startTime = time.time()
|
||
deviceData['status']['flagPressureStable'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
BTV_ReturnToZero()
|
||
if conrtrolMode == 1:
|
||
#执行回零加压操作
|
||
PreloadPressure(targetPressure)
|
||
optStatus = 0
|
||
else:
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
optStatus = 5
|
||
case _: #8，禁用电机，退出函数
|
||
deviceData['status']['flagPressureStable'] = False
|
||
_G.set(devInfo['name'], deviceData)
|
||
ret = exeCmd(['0406 0080 0000', 8])
|
||
exeFlag = False
|
||
log_du(f"optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
log(f"BTV_ControlPressure stopped.")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in BTV_ControlPressure(): optStatus={optStatus}, StopAll={deviceData['status']['StopAll']}, curPos={curPos}, curSpeed={curSpeed}, Info={Info}")
|
||
return False
|
||
|
||
def PreloadPressure(targetPressure):
|
||
global deviceData
|
||
try:
|
||
if targetPressure > 8000:
|
||
targetPressure = 8000
|
||
if BTV_IsAtZero():
|
||
PPSOV_Open()
|
||
if int(deviceData['data']['pressure']['value']) > targetPressure:
|
||
LRSOV_Open()
|
||
LRSOV_Close()
|
||
else:
|
||
PPSOV_Open()
|
||
LRSOV_Open()
|
||
BTV_ReturnToZero()
|
||
LRSOV_Close()
|
||
optStatus = 0
|
||
exeFlag = True
|
||
Info = ''
|
||
curSpeed = None
|
||
while(exeFlag):
|
||
deviceData = _G.get(devInfo['name'])
|
||
if deviceData and 'status' in deviceData and 'StopAll' in deviceData['status']:
|
||
if deviceData['status']['StopAll']:
|
||
optStatus = -1
|
||
match optStatus:
|
||
case 0:
|
||
PPECC_ReturnToZero()
|
||
optStatus = 1
|
||
case 1:
|
||
PPECC_Push(targetPressure)
|
||
if targetPressure>1000:
|
||
if (deviceData['data']['pressure']['value'] > 0.8*targetPressure):
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
curSpeed = abs(int((deviceData['data']['pressure']['value']-targetPressure))//5)
|
||
if curSpeed > 300:
|
||
curSpeed = 300
|
||
elif targetPressure > 200:
|
||
if (deviceData['data']['pressure']['value'] > 0.85*targetPressure):
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
curSpeed = 200
|
||
elif targetPressure>100:
|
||
if (deviceData['data']['pressure']['value'] > 0.9*targetPressure):
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
curSpeed = 200
|
||
else:
|
||
if (deviceData['data']['pressure']['value'] > targetPressure-3):
|
||
optStatus = -1
|
||
continue
|
||
else:
|
||
curSpeed = 200
|
||
optStatus = 0
|
||
case _:
|
||
PPSOV_Close()
|
||
exeFlag = False
|
||
log_du(f"Current pressure is {deviceData['data']['pressure']['value']}{deviceData['data']['pressure']['unit']}, optStatus={optStatus}, curSpeed={curSpeed}")
|
||
log(f"PreloadPressure stopped. {Info}")
|
||
return True
|
||
except Exception as e:
|
||
log_e(f"Error in PreloadPressure(): {str(e)}.")
|
||
return False |