448 lines
14 KiB
Python
448 lines
14 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2025/2/14 14:43
|
|||
|
|
# @Author : hjw
|
|||
|
|
# @File : test.py
|
|||
|
|
'''
|
|||
|
|
import os
|
|||
|
|
import time
|
|||
|
|
import threading
|
|||
|
|
import serial
|
|||
|
|
import socket
|
|||
|
|
import logging
|
|||
|
|
import json
|
|||
|
|
from queue import Queue
|
|||
|
|
from abc import ABC, abstractmethod
|
|||
|
|
from collections import deque
|
|||
|
|
|
|||
|
|
# 配置日志系统
|
|||
|
|
logging.basicConfig(
|
|||
|
|
level=logging.INFO,
|
|||
|
|
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
|
|||
|
|
handlers=[
|
|||
|
|
logging.FileHandler('controller.log'),
|
|||
|
|
logging.StreamHandler()
|
|||
|
|
]
|
|||
|
|
)
|
|||
|
|
logger = logging.getLogger(__name__)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def parse_config(file_path):
|
|||
|
|
config = {}
|
|||
|
|
current_level = [config] # 使用栈来跟踪当前层级的字典
|
|||
|
|
indent_stack = [] # 记录缩进层级
|
|||
|
|
|
|||
|
|
with open(file_path, 'r', encoding='utf-8') as file:
|
|||
|
|
for line in file:
|
|||
|
|
line = line.rstrip() # 去掉行尾的换行符和空格
|
|||
|
|
if not line or line.startswith('#'): # 跳过空行和注释
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
# 检测缩进层级
|
|||
|
|
indent = len(line) - len(line.lstrip())
|
|||
|
|
while indent_stack and indent <= indent_stack[-1]:
|
|||
|
|
current_level.pop()
|
|||
|
|
indent_stack.pop()
|
|||
|
|
|
|||
|
|
key, value = line.split(':', 1) # 分割键和值
|
|||
|
|
key = key.strip()
|
|||
|
|
value = value.strip()
|
|||
|
|
|
|||
|
|
# 处理值
|
|||
|
|
if value.startswith('[') and value.endswith(']'): # 列表
|
|||
|
|
value = [int(x.strip()) for x in value[1:-1].split(',')]
|
|||
|
|
elif value.isdigit(): # 数字
|
|||
|
|
value = int(value)
|
|||
|
|
elif value.replace('.', '', 1).isdigit(): # 浮点数
|
|||
|
|
value = float(value)
|
|||
|
|
elif value.lower() in ('true', 'false'): # 布尔值
|
|||
|
|
value = value.lower() == 'true'
|
|||
|
|
|
|||
|
|
# 插入到当前层级的字典中
|
|||
|
|
if value: # 如果有值，则直接赋值
|
|||
|
|
current_level[-1][key] = value
|
|||
|
|
else: # 如果没有值，则创建一个新字典
|
|||
|
|
new_dict = {}
|
|||
|
|
current_level[-1][key] = new_dict
|
|||
|
|
current_level.append(new_dict)
|
|||
|
|
indent_stack.append(indent)
|
|||
|
|
|
|||
|
|
return config
|
|||
|
|
|
|||
|
|
|
|||
|
|
class GPIOManager:
|
|||
|
|
def __init__(self, config):
|
|||
|
|
self.pins = config['gpio']['pins']
|
|||
|
|
self.pulse_per_rev = config['gpio']['pulse_per_rev']
|
|||
|
|
self.threads = {}
|
|||
|
|
self.stop_flags = {}
|
|||
|
|
self.gpio_files = {}
|
|||
|
|
self.lock = threading.Lock()
|
|||
|
|
|
|||
|
|
for pin in self.pins:
|
|||
|
|
self._init_gpio(pin)
|
|||
|
|
|
|||
|
|
def _init_gpio(self, pin):
|
|||
|
|
try:
|
|||
|
|
export_path = '/sys/class/gpio/export'
|
|||
|
|
gpio_path = f'/sys/class/gpio/gpio{pin}'
|
|||
|
|
|
|||
|
|
if not os.path.exists(gpio_path):
|
|||
|
|
with open(export_path, 'w') as f:
|
|||
|
|
f.write(str(pin))
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
direction_path = f'{gpio_path}/direction'
|
|||
|
|
with open(direction_path, 'w') as f:
|
|||
|
|
f.write('out')
|
|||
|
|
|
|||
|
|
self.gpio_files[pin] = open(f'{gpio_path}/value', 'r+')
|
|||
|
|
logger.info(f"GPIO {pin} 初始化完成")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"GPIO {pin} 初始化失败: {str(e)}")
|
|||
|
|
raise
|
|||
|
|
|
|||
|
|
def set_speed(self, pin_id, speed):
|
|||
|
|
with self.lock:
|
|||
|
|
if pin_id not in self.pins:
|
|||
|
|
raise ValueError(f"无效的GPIO引脚: {pin_id}")
|
|||
|
|
|
|||
|
|
# 停止现有线程
|
|||
|
|
if pin_id in self.threads:
|
|||
|
|
self.stop_flags[pin_id] = True
|
|||
|
|
self.threads[pin_id].join()
|
|||
|
|
|
|||
|
|
if speed <= 0:
|
|||
|
|
self._write_value(pin_id, 0)
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 计算间隔时间（400脉冲/转）
|
|||
|
|
interval = 1.0 / (2 * speed) # 每个周期包含高低电平
|
|||
|
|
self.stop_flags[pin_id] = False
|
|||
|
|
self.threads[pin_id] = threading.Thread(
|
|||
|
|
target=self._pulse_loop,
|
|||
|
|
args=(pin_id, interval),
|
|||
|
|
daemon=True
|
|||
|
|
)
|
|||
|
|
self.threads[pin_id].start()
|
|||
|
|
|
|||
|
|
def _pulse_loop(self, pin_id, interval):
|
|||
|
|
while not self.stop_flags.get(pin_id, False):
|
|||
|
|
self._write_value(pin_id, 1)
|
|||
|
|
time.sleep(interval)
|
|||
|
|
self._write_value(pin_id, 0)
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
def _write_value(self, pin_id, value):
|
|||
|
|
try:
|
|||
|
|
self.gpio_files[pin_id].seek(0)
|
|||
|
|
self.gpio_files[pin_id].write(str(value))
|
|||
|
|
self.gpio_files[pin_id].truncate()
|
|||
|
|
self.gpio_files[pin_id].flush()
|
|||
|
|
os.fsync(self.gpio_files[pin_id].fileno())
|
|||
|
|
except IOError as e:
|
|||
|
|
logger.error(f"GPIO写入错误: {str(e)}")
|
|||
|
|
|
|||
|
|
def cleanup(self):
|
|||
|
|
with self.lock:
|
|||
|
|
for pin in self.pins:
|
|||
|
|
if pin in self.threads:
|
|||
|
|
self.stop_flags[pin] = True
|
|||
|
|
self.threads[pin].join()
|
|||
|
|
self._write_value(pin, 0)
|
|||
|
|
for f in self.gpio_files.values():
|
|||
|
|
f.close()
|
|||
|
|
logger.info("GPIO资源已释放")
|
|||
|
|
|
|||
|
|
|
|||
|
|
class RS485Reader:
|
|||
|
|
def __init__(self, config):
|
|||
|
|
self.port = config['serial']['port']
|
|||
|
|
self.baudrate = config['serial']['baudrate']
|
|||
|
|
self.ser = None
|
|||
|
|
self.current_weight = 0
|
|||
|
|
self.running = True
|
|||
|
|
self.lock = threading.Lock()
|
|||
|
|
self.buffer = deque(maxlen=10) # 数据缓冲
|
|||
|
|
self._connect()
|
|||
|
|
|
|||
|
|
def _connect(self):
|
|||
|
|
try:
|
|||
|
|
self.ser = serial.Serial(
|
|||
|
|
port=self.port,
|
|||
|
|
baudrate=self.baudrate,
|
|||
|
|
bytesize=8,
|
|||
|
|
parity='N',
|
|||
|
|
stopbits=1,
|
|||
|
|
timeout=0.1
|
|||
|
|
)
|
|||
|
|
logger.info(f"成功连接串口设备 {self.port}")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"串口连接失败: {str(e)}")
|
|||
|
|
raise
|
|||
|
|
|
|||
|
|
def read_weight(self):
|
|||
|
|
cmd = bytes.fromhex('010300000002c40b')
|
|||
|
|
try:
|
|||
|
|
self.ser.write(cmd)
|
|||
|
|
response = self.ser.read(9)
|
|||
|
|
if len(response) == 9 and response[:3] == bytes.fromhex("010304"):
|
|||
|
|
weight = int.from_bytes(response[3:5], 'big')
|
|||
|
|
with self.lock:
|
|||
|
|
self.buffer.append(weight)
|
|||
|
|
self.current_weight = sum(self.buffer) // len(self.buffer)
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"读取重量失败: {str(e)}")
|
|||
|
|
self._reconnect()
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def _reconnect(self):
|
|||
|
|
max_retries = 3
|
|||
|
|
for i in range(max_retries):
|
|||
|
|
try:
|
|||
|
|
if self.ser:
|
|||
|
|
self.ser.close()
|
|||
|
|
self._connect()
|
|||
|
|
return True
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.warning(f"重连尝试 {i + 1}/{max_retries} 失败")
|
|||
|
|
time.sleep(1)
|
|||
|
|
logger.error("串口重连失败")
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
def stop(self):
|
|||
|
|
self.running = False
|
|||
|
|
if self.ser and self.ser.is_open:
|
|||
|
|
self.ser.close()
|
|||
|
|
|
|||
|
|
|
|||
|
|
class ControlAlgorithm(ABC):
|
|||
|
|
@abstractmethod
|
|||
|
|
def calculate_speed(self, current, target):
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
class PIDAlgorithm(ControlAlgorithm):
|
|||
|
|
def __init__(self, kp=0.5, ki=0.01, kd=0.1):
|
|||
|
|
self.kp = kp
|
|||
|
|
self.ki = ki
|
|||
|
|
self.kd = kd
|
|||
|
|
self.integral = 0
|
|||
|
|
self.last_error = 0
|
|||
|
|
self.min_speed = 10 # 最小运行速度
|
|||
|
|
self.max_speed = 100 # 最大运行速度
|
|||
|
|
|
|||
|
|
def calculate_speed(self, current, target):
|
|||
|
|
error = target - current
|
|||
|
|
self.integral += error
|
|||
|
|
derivative = error - self.last_error
|
|||
|
|
self.last_error = error
|
|||
|
|
|
|||
|
|
# 抗积分饱和
|
|||
|
|
if self.integral > 1000:
|
|||
|
|
self.integral = 1000
|
|||
|
|
elif self.integral < -1000:
|
|||
|
|
self.integral = -1000
|
|||
|
|
|
|||
|
|
output = self.kp * error + self.ki * self.integral + self.kd * derivative
|
|||
|
|
#误差5g 5*0.5 + 1000* 0.01 + [0.5*5] = 2.5 + 10 + ... = 12.5
|
|||
|
|
#误差500g 500*0.5 + 1000* 0.01 + [0.5*10] = 250 + 10 + ... = 260
|
|||
|
|
return max(self.min_speed, min(self.max_speed, output)) #
|
|||
|
|
|
|||
|
|
|
|||
|
|
class FuzzyLogicAlgorithm(ControlAlgorithm):
|
|||
|
|
def calculate_speed(self, current, target):
|
|||
|
|
error = target - current
|
|||
|
|
abs_error = abs(error)
|
|||
|
|
|
|||
|
|
if abs_error > 100:
|
|||
|
|
return 100
|
|||
|
|
elif abs_error > 50:
|
|||
|
|
return 70
|
|||
|
|
elif abs_error > 20:
|
|||
|
|
return 50
|
|||
|
|
else:
|
|||
|
|
return 30
|
|||
|
|
|
|||
|
|
|
|||
|
|
class NetworkHandler:
|
|||
|
|
def __init__(self, config):
|
|||
|
|
self.host = config['network']['host']
|
|||
|
|
self.port = config['network']['port']
|
|||
|
|
self.sock = None
|
|||
|
|
self.command_queue = Queue()
|
|||
|
|
self.status = {
|
|||
|
|
'measuring': False,
|
|||
|
|
'error': None,
|
|||
|
|
'current_weight': 0,
|
|||
|
|
'target_weight': 0,
|
|||
|
|
'algorithm': 'pid'
|
|||
|
|
}
|
|||
|
|
self.lock = threading.Lock()
|
|||
|
|
self.running = True
|
|||
|
|
|
|||
|
|
def start_server(self):
|
|||
|
|
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|||
|
|
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|||
|
|
try:
|
|||
|
|
self.sock.bind((self.host, self.port))
|
|||
|
|
self.sock.listen(5)
|
|||
|
|
logger.info(f"网络服务启动于 {self.host}:{self.port}")
|
|||
|
|
|
|||
|
|
while self.running:
|
|||
|
|
conn, addr = self.sock.accept()
|
|||
|
|
threading.Thread(target=self._handle_client, args=(conn, addr), daemon=True).start()
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"网络服务异常: {str(e)}")
|
|||
|
|
finally:
|
|||
|
|
self.sock.close()
|
|||
|
|
|
|||
|
|
def _handle_client(self, conn, addr):
|
|||
|
|
try:
|
|||
|
|
data = conn.recv(1024)
|
|||
|
|
if data:
|
|||
|
|
cmd = json.loads(data.decode())
|
|||
|
|
self._process_command(cmd)
|
|||
|
|
|
|||
|
|
# 返回当前状态
|
|||
|
|
with self.lock:
|
|||
|
|
response = json.dumps(self.status)
|
|||
|
|
conn.send(response.encode())
|
|||
|
|
except json.JSONDecodeError:
|
|||
|
|
logger.warning("收到无效的JSON指令")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"客户端处理异常: {str(e)}")
|
|||
|
|
finally:
|
|||
|
|
conn.close()
|
|||
|
|
|
|||
|
|
def _process_command(self, cmd):
|
|||
|
|
with self.lock:
|
|||
|
|
if cmd.get('command') == 'set_target':
|
|||
|
|
|
|||
|
|
self.status['target_weight'] = cmd['payload']['target_weight']
|
|||
|
|
self.status['algorithm'] = cmd['payload'].get('algorithm', 'pid')
|
|||
|
|
self.status['measuring'] = True
|
|||
|
|
print("收到指令set_target:", self.status['target_weight'])
|
|||
|
|
elif cmd.get('command') == 'stop':
|
|||
|
|
self.status['measuring'] = False
|
|||
|
|
|
|||
|
|
|
|||
|
|
class Controller:
|
|||
|
|
def __init__(self, config_path='config.yaml'):
|
|||
|
|
|
|||
|
|
self.config = parse_config(config_path)
|
|||
|
|
|
|||
|
|
# 初始化组件
|
|||
|
|
self.gpio = GPIOManager(self.config)
|
|||
|
|
self.sensor = RS485Reader(self.config)
|
|||
|
|
self.network = NetworkHandler(self.config)
|
|||
|
|
self.algorithm = None
|
|||
|
|
self.running = False
|
|||
|
|
|
|||
|
|
# 共享状态
|
|||
|
|
self.current_weight = 0
|
|||
|
|
self.target_weight = 0
|
|||
|
|
self.system_status = 'idle'
|
|||
|
|
self.status_lock = threading.Lock()
|
|||
|
|
|
|||
|
|
# 看门狗线程
|
|||
|
|
self.watchdog = threading.Thread(target=self._watchdog, daemon=True)
|
|||
|
|
|
|||
|
|
def start(self):
|
|||
|
|
self.running = True
|
|||
|
|
threads = [
|
|||
|
|
threading.Thread(target=self._weight_reading_loop),
|
|||
|
|
threading.Thread(target=self._control_loop),
|
|||
|
|
threading.Thread(target=self.network.start_server),
|
|||
|
|
self.watchdog
|
|||
|
|
]
|
|||
|
|
for t in threads:
|
|||
|
|
t.start()
|
|||
|
|
logger.info("系统启动完成")
|
|||
|
|
|
|||
|
|
def _weight_reading_loop(self):
|
|||
|
|
while self.running:
|
|||
|
|
if self.sensor.read_weight():
|
|||
|
|
with self.sensor.lock:
|
|||
|
|
self.current_weight = self.sensor.current_weight
|
|||
|
|
with self.status_lock:
|
|||
|
|
self.network.status['current_weight'] = self.current_weight
|
|||
|
|
if self.current_weight >= self.network.status['target_weight']:
|
|||
|
|
self.network.status['measuring'] = False
|
|||
|
|
time.sleep(0.05) # 20Hz
|
|||
|
|
#time.sleep(0.1) # 20Hz
|
|||
|
|
|
|||
|
|
def _control_loop(self):
|
|||
|
|
while self.running:
|
|||
|
|
with self.status_lock:
|
|||
|
|
measuring = self.network.status['measuring']
|
|||
|
|
target = self.network.status['target_weight']
|
|||
|
|
algorithm = self.network.status['algorithm']
|
|||
|
|
|
|||
|
|
if measuring:
|
|||
|
|
self._select_algorithm(algorithm)
|
|||
|
|
speed = self.algorithm.calculate_speed(self.current_weight, target)
|
|||
|
|
rpm = speed # 假设速度单位直接对应RPM
|
|||
|
|
pulse_rate = (int(self.config['gpio']['pulse_per_rev']) * rpm) / 60
|
|||
|
|
self.gpio.set_speed(8, pulse_rate)
|
|||
|
|
else:
|
|||
|
|
self.gpio.set_speed(8, 0)
|
|||
|
|
while not self.running and not self.network.status['measuring']:
|
|||
|
|
time.sleep(0.1) # 短暂休眠，避免CPU占用过高
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
def _select_algorithm(self, name):
|
|||
|
|
if name == 'pid':
|
|||
|
|
params = self.config['algorithm']['params']['pid']
|
|||
|
|
self.algorithm = PIDAlgorithm(**params)
|
|||
|
|
elif name == 'fuzzy':
|
|||
|
|
self.algorithm = FuzzyLogicAlgorithm()
|
|||
|
|
else:
|
|||
|
|
self.algorithm = PIDAlgorithm()
|
|||
|
|
|
|||
|
|
def _watchdog(self):
|
|||
|
|
while self.running:
|
|||
|
|
# 检查线程状态
|
|||
|
|
threads_alive = all([
|
|||
|
|
threading.current_thread().is_alive(),
|
|||
|
|
self.sensor.running,
|
|||
|
|
self.network.running
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
if not threads_alive:
|
|||
|
|
logger.critical("检测到线程异常，触发紧急停止！")
|
|||
|
|
self.emergency_stop()
|
|||
|
|
break
|
|||
|
|
|
|||
|
|
time.sleep(5)
|
|||
|
|
|
|||
|
|
def emergency_stop(self):
|
|||
|
|
with self.status_lock:
|
|||
|
|
self.network.status['measuring'] = False
|
|||
|
|
self.network.status['error'] = 'emergency_stop'
|
|||
|
|
self.gpio.cleanup()
|
|||
|
|
self.sensor.stop()
|
|||
|
|
self.running = False
|
|||
|
|
logger.info("系统已紧急停止")
|
|||
|
|
|
|||
|
|
def shutdown(self):
|
|||
|
|
self.running = False
|
|||
|
|
self.gpio.cleanup()
|
|||
|
|
self.sensor.stop()
|
|||
|
|
self.network.running = False
|
|||
|
|
logger.info("系统正常关闭")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
ctrl = Controller()
|
|||
|
|
try:
|
|||
|
|
ctrl.start()
|
|||
|
|
while True:
|
|||
|
|
time.sleep(1)
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
logger.info("收到停止信号...")
|
|||
|
|
finally:
|
|||
|
|
ctrl.shutdown()
|