ShunFang-cart/PekingGripper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init repo

Browse Source
This commit is contained in:
Brunsmeier
2026-07-16 14:56:43 +08:00
commit 111c9bab8a
25 changed files with 4289 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

199
cap_read.py Normal file
View File

@ -0,0 +1,199 @@
from enum import *
import threading
import queue # 导入 queue 模块
from class_ch341 import *
from class_sensorcmd import *
from class_finger import *
from collections import namedtuple
import socket
DEF_CDC_SYNC_MS = 1000 #电容同步间隔
DEF_GET_CAP_MS = (30) #读取电容间隔
DEF_PRO_CYC = 100
DEF_MAX_FINGER_NUM = 1 #需要连接的手指数量，最大5个
# 定义一个全局的队列，用于线程间通信
capReadQueue = queue.Queue()
# 341通信
class EnumCh341ConnectStatus(Enum):
CH341_CONNECT_INIT = 0
CH341_CONNECT_OPEN = 1
CH341_CONNECT_SET_SPEED = 2
CH341_CONNECT_SAMPLE_START = 3
CH341_CONNECT_CHECK = 4
CH341_CONNECT_SAMPLE_STOP = 5
class ClassCapRead:
def __init__(self):
self.ch341 = ClassCh341()
# 最大连接5个手指
self.fingers = list() # 传感器列表
for i in range(DEF_MAX_FINGER_NUM):
self.fingers.append(ClassFinger(2+i, self.ch341))
self.currCh341State = 0 # 当前ch341连接状态
self.prevCh341State = 0 # 上次ch341连接状态
self.ch341CheckTimer = 0
self.mcuInit = 0
self.pcaAddr = 0x70 # iic控制芯片地址
self.ch341Init = 0 # ch341初始化标志位
self.syncTimer = 0
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
self.connectDebug()
def __del__(self):
self.disConnectDebug()
self.ch341.disconnect()
print("ch341释放")
pass
def connectDebug(self):
#连接到调试的服务器
self.vofaClient = socket.socket()
addr = ('127.0.0.1', 1347)
try:
self.vofaClient.connect(addr)
# client.send('hello world\r\n'.encode())
self.socketConnected = True
print('连接服务器成功')
except Exception as e:
self.socketConnected = False
print('连接服务器失败')
def disConnectDebug(self):
if self.vofaClient:
self.vofaClient.close()
def debugPrint(self):
if self.socketConnected == True:
fingerIndex = 0
_log1 = ""
# 输出原始通道数值
# for index in range(0, self.fingers[fingerIndex].projectPara.sensor_num):
# _log1 += str(self.fingers[fingerIndex].readData.channelCapData[index])
# _log1 += ','
for index in range(0, self.fingers[fingerIndex].projectPara.ydds_num):
_log1 += str(int(self.fingers[fingerIndex].readData.nf[index]*1000))
_log1 += ','
_log1 += str(int(self.fingers[fingerIndex].readData.tf[index]*1000))
_log1 += ','
_log1 += str(self.fingers[fingerIndex].readData.tfDir[index])
_log1 += ','
for index in range(0, self.fingers[fingerIndex].projectPara.s_prox_num):
_log1 += str(self.fingers[fingerIndex].readData.sProxCapData[index])
_log1 += ','
for index in range(0, self.fingers[fingerIndex].projectPara.m_prox_num):
_log1 += str(self.fingers[fingerIndex].readData.mProxCapData[index])
_log1 += ','
_log1 += str(0)
_log1 += '\r\n'
#print(_log1)
if self.socketConnected == 1:
self.vofaClient.send(_log1.encode())
def set_sensor_enable(self, idx):
_pack = list()
_pack.append(idx)
self.ch341.write(self.pcaAddr, _pack)
def ch341Connect(self):
if self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_INIT:
if(True == self.ch341.init()):
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_OPEN
elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_OPEN:
if(True == self.ch341.open()):
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SET_SPEED
else:
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SET_SPEED:
if(True == self.ch341.set_speed(self.ch341.IIC_SPEED_400)):
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START
else:
print("set speed err")
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START
elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START:
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_CHECK
self.timer = threading.Timer(DEF_GET_CAP_MS/1000, self.capRead)
self.timer.start()
elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_CHECK:
self.ch341CheckTimer += DEF_PRO_CYC
if(self.ch341CheckTimer >= 1000):
self.ch341CheckTimer = 0
if(False == self.ch341.connectCheck()):
print("ch341 拔出")
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_STOP
elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_STOP:
self.syncTimer = 0
for i in range(0, len(self.fingers)):
self.fingers[i].disconnected()
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
else:
self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
def capRead(self):
capReadTime = time.time()
ms_capReadTime = capReadTime
connectedSensorChan = 0
connectedSensorCnt = 0
for fingerIndex in range(0, len(self.fingers)):
self.set_sensor_enable(1 << (self.fingers[fingerIndex].pcaIdx))
connectedSensorChan |= 1 << (self.fingers[fingerIndex].pcaIdx)
if self.fingers[fingerIndex].connect == False:
if True == self.fingers[fingerIndex].checkSensor():
print(f"sensor[{fingerIndex}] connected")
else:
print(f"addr = {fingerIndex}, connected false")
else:
self.fingers[fingerIndex].capRead()
connectedSensorCnt += 1
self.debugPrint()
# 大于1个传感器连接需要设置接近采集序列
if connectedSensorCnt > 1 and (time.time() - self.syncTimer) > DEF_CDC_SYNC_MS:
self.syncTimer = time.time()
self.set_sensor_enable(connectedSensorChan)
for fingerIndex in range(0, len(self.fingers)):
if self.fingers[fingerIndex].connect is True:
self.fingers[fingerIndex].snsCmd.setSensorSync(0)
break
if self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_CHECK:
capReadTime = time.time()
difftime = int(capReadTime*1000-ms_capReadTime*1000)
#print(f"diffTime={difftime}")
#定时器在任务完成后重新启动
if(difftime>DEF_GET_CAP_MS):
timer = threading.Timer(DEF_GET_CAP_MS/1000, self.capRead)
else:
timer = threading.Timer((DEF_GET_CAP_MS-difftime)/1000, self.capRead)
timer.start()
def capReadThread():
# 线程的主体功能
cap = ClassCapRead()
while True:
cap.ch341Connect()
time.sleep(DEF_PRO_CYC/1000)
def main():
capReadThread()
if __name__ == "__main__":
main()