import sys
import math
import csv
import serial
import time
import numpy as np
from typing import List
from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, QHBoxLayout,
QPushButton, QLabel, QLineEdit, QTextEdit, QFileDialog)
from PyQt5.QtCore import QThread, pyqtSignal, Qt
import matplotlib.pyplot as plt
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
# 设置Matplotlib中文字体
plt.rcParams['font.sans-serif'] = ['SimHei'] # 使用黑体显示中文
plt.rcParams['axes.unicode_minus'] = False # 正确显示负号
class AngleCalibrator:
def __init__(self, com_port: str = 'COM3', baudrate: int = 9600):
self.serial_config = {
'port': com_port,
'baudrate': baudrate,
'timeout': 1
}
# PID参数
self.Kp = 1.0 # 比例系数
self.Ki = 0.1 # 新增积分系数
self.Kd = 0.05 # 新增微分系数
self.tolerance = 0.5 # 角度容差(度)
self.integral_max = 10.0 # 积分限幅
self.last_error = 0.0 # 上次误差
self.integral = 0.0 # 积分项
def read_expected_angles(self, csv_path: str) -> List[float]:
"""读取CSV文件中的目标角度数据"""
angles = []
try:
with open(csv_path, 'r', encoding='utf-8-sig') as f:
reader = csv.reader(f)
next(reader) # 跳过标题行
for row in reader:
if len(row) >= 2:
try:
angles.append(float(row[1]))
except ValueError:
print(f"无效数据行:{row}")
return angles
except Exception as e:
print(f"文件读取失败:{str(e)}")
return []
def _send_current(self, z: float, x: float) -> bool:
"""向电源发送电流指令(核心方法)"""
try:
with serial.Serial(**self.serial_config) as ser:
# 设备初始化
ser.write(b'*CLS\n') # 清除状态
ser.write(b':SYST:REM\n') # 设置远程控制
time.sleep(0.1)
# 发送电流指令
commands = [
f':SOUR1:CURR {z:.4f}\n'.encode(),
f':SOUR2:CURR {x:.4f}\n'.encode(),
b':OUTP ON\n' # 开启输出
]
# 执行指令并验证
success = True
for cmd in commands:
ser.write(cmd)
time.sleep(0.1)
if ser.in_waiting:
response = ser.read(ser.in_waiting).decode()
if 'ERROR' in response:
print(f"指令错误:{cmd.decode().strip()}")
success = False
return success
except serial.SerialException as e:
print(f"串口连接异常:{str(e)}")
return False
except Exception as e:
print(f"意外错误:{str(e)}")
return False
def _compute_components(self, angle: float) -> tuple:
"""计算指定角度的电流分量"""
rad = math.radians(angle)
z = max(-1.0, min(1.0, math.cos(rad)))
x = max(-1.0, min(1.0, math.sin(rad)))
return z, x
class CalibrationThread(QThread):
update_signal = pyqtSignal(float, float, float, float, float) # 目标角度,当前角度,误差,Z电流,X电流
status_signal = pyqtSignal(str)
finished_signal = pyqtSignal()
error_request = pyqtSignal()
def __init__(self, calibrator, csv_path):
super().__init__()
self.calibrator = calibrator
self.csv_path = csv_path
self.is_paused = False
self.is_stopped = False
self.current_error = None
self.current_target = None
def run(self):
angles = self.calibrator.read_expected_angles(self.csv_path)
if not angles:
self.status_signal.emit("无有效角度数据")
return
for target in angles:
if self.is_stopped:
break
self.current_target = target
current_angle = target
z, x = self.calibrator._compute_components(current_angle)
self.update_signal.emit(target, current_angle, 0, z, x)
# 重置PID状态
self.calibrator.last_error = 0.0
self.calibrator.integral = 0.0
last_time = time.time()
while not self.is_stopped:
if self.is_paused:
self.msleep(100)
continue
# 请求误差输入
self.error_request.emit()
while self.current_error is None and not self.is_stopped:
self.msleep(100)
if self.is_stopped:
break
error = self.current_error
self.current_error = None
# 计算时间差
now = time.time()
dt = now - last_time
last_time = now
# PID计算
# 比例项
P = self.calibrator.Kp * error
# 积分项(带限幅)
self.calibrator.integral += error * dt
self.calibrator.integral = max(-self.calibrator.integral_max,
min(self.calibrator.integral_max,
self.calibrator.integral))
I = self.calibrator.Ki * self.calibrator.integral
# 微分项
D = self.calibrator.Kd * (error - self.calibrator.last_error) / dt
self.calibrator.last_error = error
# 总调整量
adjustment = P + I + D
# 计算实际角度
actual = current_angle - error
self.update_signal.emit(target, actual, error, *self.calibrator._compute_components(current_angle))
# 达标判断
if abs(actual - target) <= self.calibrator.tolerance:
self.status_signal.emit(f"角度 {target}° 校准完成")
break
# 计算新理论角度
new_angle = current_angle + adjustment
z_new, x_new = self.calibrator._compute_components(new_angle)
# 发送新电流值
if self.calibrator._send_current(z_new, x_new):
current_angle = new_angle
self.update_signal.emit(target, current_angle, error, z_new, x_new)
self.status_signal.emit(f"PID调整到新角度:{new_angle:.2f}°")
else:
self.status_signal.emit("电流调整失败")
self.finished_signal.emit()
def set_error(self, error):
self.current_error = error
class CalibrationGUI(QMainWindow):
def __init__(self):
super().__init__()
self.calibrator = AngleCalibrator()
self.thread = None
self.current_csv_path = None
self.init_ui()
self.init_plot()
self.reset_data()
def reset_data(self):
self.time_data = []
self.target_data = []
self.actual_data = []
def init_ui(self):
self.setWindowTitle("角度校准系统")
self.setGeometry(100, 100, 1000, 800)
main_widget = QWidget()
layout = QHBoxLayout()
# 控制面板
control_panel = QVBoxLayout()
self.btn_start = QPushButton("开始校准")
self.btn_restart = QPushButton("重新校准")
self.btn_pause = QPushButton("暂停")
self.btn_stop = QPushButton("停止")
self.btn_start.clicked.connect(self.start_calibration)
self.btn_restart.clicked.connect(self.restart_calibration)
self.btn_pause.clicked.connect(self.toggle_pause)
self.btn_stop.clicked.connect(self.stop_calibration)
self.error_input = QLineEdit()
self.btn_confirm = QPushButton("提交误差")
self.btn_confirm.clicked.connect(self.confirm_error)
self.status_display = QTextEdit()
self.status_display.setReadOnly(True)
control_panel.addWidget(self.btn_start)
control_panel.addWidget(self.btn_restart)
control_panel.addWidget(self.btn_pause)
control_panel.addWidget(self.btn_stop)
control_panel.addSpacing(20)
control_panel.addWidget(QLabel("输入误差(°):"))
control_panel.addWidget(self.error_input)
control_panel.addWidget(self.btn_confirm)
control_panel.addSpacing(20)
control_panel.addWidget(QLabel("状态信息:"))
control_panel.addWidget(self.status_display)
# 可视化面板
vis_panel = QVBoxLayout()
self.figure = plt.figure()
self.canvas = FigureCanvas(self.figure)
vis_panel.addWidget(self.canvas)
# 数值显示
info_layout = QVBoxLayout()
self.lbl_target = QLabel("目标角度:--")
self.lbl_current = QLabel("当前角度:--")
self.lbl_error = QLabel("测量误差:--")
self.lbl_current_z = QLabel("Z轴电流:--")
self.lbl_current_x = QLabel("X轴电流:--")
for widget in [self.lbl_target, self.lbl_current, self.lbl_error,
self.lbl_current_z, self.lbl_current_x]:
info_layout.addWidget(widget)
vis_panel.addLayout(info_layout)
layout.addLayout(control_panel, 1)
layout.addLayout(vis_panel, 2)
main_widget.setLayout(layout)
self.setCentralWidget(main_widget)
self.btn_pause.setEnabled(False)
self.btn_stop.setEnabled(False)
self.btn_confirm.setEnabled(False)
self.btn_restart.setEnabled(False)
def init_plot(self):
self.ax = self.figure.add_subplot(111)
self.ax.set_title("角度变化趋势")
self.ax.set_xlabel("调整次数")
self.ax.set_ylabel("角度(°)")
self.line_target, = self.ax.plot([], [], 'r-', label='目标角度')
self.line_actual, = self.ax.plot([], [], 'b--', label='实际角度')
self.ax.legend()
self.canvas.draw()
def start_calibration(self, path=None):
if not path:
path, _ = QFileDialog.getOpenFileName(self, "选择CSV文件", "", "CSV文件 (*.csv)")
if not path:
return
self.current_csv_path = path
self.reset_data()
self.thread = CalibrationThread(self.calibrator, self.current_csv_path)
self.thread.update_signal.connect(self.update_display)
self.thread.status_signal.connect(self.show_status)
self.thread.finished_signal.connect(self.on_finished)
self.thread.error_request.connect(self.request_error_input)
self.thread.start()
self.btn_start.setEnabled(False)
self.btn_restart.setEnabled(True)
self.btn_pause.setEnabled(True)
self.btn_stop.setEnabled(True)
self.btn_confirm.setEnabled(False)
def restart_calibration(self):
if self.current_csv_path:
self.stop_calibration()
self.start_calibration(self.current_csv_path)
def toggle_pause(self):
if self.thread:
self.thread.is_paused = not self.thread.is_paused
self.btn_pause.setText("恢复" if self.thread.is_paused else "暂停")
self.show_status("已暂停" if self.thread.is_paused else "已恢复")
def stop_calibration(self):
if self.thread:
self.thread.is_stopped = True
self.on_finished()
def request_error_input(self):
self.btn_confirm.setEnabled(True)
self.error_input.setFocus()
self.show_status("请测量并输入当前误差")
def confirm_error(self):
try:
error = float(self.error_input.text())
self.thread.set_error(error)
self.error_input.clear()
self.btn_confirm.setEnabled(False)
except ValueError:
self.show_status("错误:请输入有效数字")
def update_display(self, target, current, error, z, x):
self.lbl_target.setText(f"目标角度:{target:.2f}°")
self.lbl_current.setText(f"当前角度:{current:.2f}°")
self.lbl_error.setText(f"测量误差:{error:.2f}°")
self.lbl_current_z.setText(f"Z轴电流:{z:.4f}A")
self.lbl_current_x.setText(f"X轴电流:{x:.4f}A")
self.time_data.append(len(self.time_data))
self.target_data.append(target)
self.actual_data.append(current)
self.line_target.set_data(self.time_data, self.target_data)
self.line_actual.set_data(self.time_data, self.actual_data)
self.ax.relim()
self.ax.autoscale_view()
self.canvas.draw()
def show_status(self, message):
self.status_display.append(f"[{time.strftime('%H:%M:%S')}] {message}")
def on_finished(self):
self.btn_start.setEnabled(True)
self.btn_restart.setEnabled(True)
self.btn_pause.setEnabled(False)
self.btn_stop.setEnabled(False)
self.btn_confirm.setEnabled(False)
self.show_status("校准过程已完成")
if __name__ == "__main__":
app = QApplication(sys.argv)
window = CalibrationGUI()
window.show()
sys.exit(app.exec_())上述代码在纠正信号时,纠正信号减去误差信号的实际偏转角度比预期角度大的话继续校准发现他纠正角度还在变大,按理说应该减小纠正信号来使实际角度靠近预期角度。应该如何修改代码,给我修改后的完整代码
最新发布