Added editable text fields for PID gains

Author: oravla5

autotune/autotune.py

@@ -37,7 +37,7 @@
 """
 
 import sys
-from PyQt5.QtWidgets import QDialog, QApplication, QLabel, QRadioButton, QSlider, QPushButton, QVBoxLayout, QHBoxLayout, QFormLayout, QFileDialog, QLineEdit, QSpinBox, QDoubleSpinBox, QMessageBox, QCheckBox, QTableWidget, QTableWidgetItem, QHeaderView, QGroupBox, QComboBox, QTabWidget, QWidget, QGridLayout
+from PyQt5.QtWidgets import QDialog, QApplication, QLabel, QRadioButton, QSlider, QPushButton, QVBoxLayout, QHBoxLayout, QFormLayout, QFileDialog, QLineEdit, QSpinBox, QDoubleSpinBox, QMessageBox, QCheckBox, QTableWidget, QTableWidgetItem, QHeaderView, QGroupBox, QComboBox, QTabWidget, QWidget, QGridLayout, QSizePolicy
 from PyQt5.QtCore import Qt
 
 from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
@@ -53,6 +53,13 @@
 
 from data_selection_window import DataSelectionWindow
 
+def isNumber(value):
+    try:
+        float(value)
+        return True
+    except ValueError:
+        return False
+
 class Window(QDialog):
     def __init__(self, parent=None):
         super(Window, self).__init__(parent)
@@ -70,10 +77,12 @@ def __init__(self, parent=None):
         self.rise_time = 0.13
         self.damping_index = 0.0
         self.detune_coeff = 0.5
-        self.kc = 0.01
-        self.ki = 0.0
-        self.kd = 0.0
-        self.kff = 0.0
+        self.gains = {
+            "P": 0.01,
+            "I": 0.0,
+            "D": 0.0,
+            "FF": 0.0
+        }
         self.figure = plt.figure(1)
         self.figure.subplots_adjust(hspace=0.5, wspace=1.0)
         self.num = []
@@ -287,6 +296,22 @@ def printImproperTfError(self):
         msg.exec_()
 
     def createPidLayout(self):
+        self.gain_line_edit = {}
+        self.gain_slider = {}
+        self.parallel_gain_lbl = {}
+        slider_props = {
+            "P": {"min": 0.001, "max": 4.0, "step": 0.001},
+            "I": {"min": 0.0, "max": 20.0, "step": 0.1},
+            "D": {"min": 0.0, "max": 0.2, "step": 0.001},
+            "FF": {"min": 0.0, "max": 1.0, "step": 0.001}
+        }
+
+        def make_slider_callback(gain):
+            return lambda: self.updateGainFromSlider(gain)
+
+        def make_line_edit_callback(gain):
+            return lambda: self.updateGainFromLineEdit(gain)
+
         layout_pid = QGridLayout()
 
         layout_options = QHBoxLayout()
@@ -304,87 +329,62 @@ def createPidLayout(self):
         layout_pid.addWidget(QLabel("Ideal/Standard\nKp * [1 + Ki + Kd]"), 0, 2)
         layout_pid.addWidget(QLabel("Parallel\nKp + Ki + Kd"), 0, 3)
 
-        layout_pid.addWidget(QLabel("K"), 1, 0)
-        self.slider_k = DoubleSlider(Qt.Horizontal)
-        self.slider_k.setMinimum(0.001)
-        self.slider_k.setMaximum(4.0)
-        self.slider_k.setInterval(0.001)
-        self.slider_k.valueChanged.connect(self.updateLabelK)
-        layout_pid.addWidget(self.slider_k, 1, 1)
-        self.lbl_k_standard = QLabel("{:.3f}".format(self.kc))
-        layout_pid.addWidget(self.lbl_k_standard, 1, 2)
-        self.lbl_k_parallel = QLabel("{:.3f}".format(self.kc))
-        layout_pid.addWidget(self.lbl_k_parallel, 1, 3)
-
-        layout_pid.addWidget(QLabel("I"), 2, 0)
-        self.slider_i = DoubleSlider(Qt.Horizontal)
-        self.slider_i.setMinimum(0.0)
-        self.slider_i.setMaximum(20.0)
-        self.slider_i.setInterval(0.1)
-        self.slider_i.valueChanged.connect(self.updateLabelI)
-        layout_pid.addWidget(self.slider_i, 2, 1)
-        self.lbl_i_standard = QLabel("{:.2f}".format(self.ki))
-        layout_pid.addWidget(self.lbl_i_standard, 2, 2)
-        self.lbl_i_parallel = QLabel("{:.2f}".format(self.kc * self.ki))
-        layout_pid.addWidget(self.lbl_i_parallel, 2, 3)
-
-        layout_pid.addWidget(QLabel("D"), 3, 0)
-        self.slider_d = DoubleSlider(Qt.Horizontal)
-        self.slider_d.setMinimum(0.0)
-        self.slider_d.setMaximum(0.2)
-        self.slider_d.setInterval(0.001)
-        self.slider_d.valueChanged.connect(self.updateLabelD)
-        layout_pid.addWidget(self.slider_d, 3, 1)
-        self.lbl_d_standard = QLabel("{:.3f}".format(self.kd))
-        layout_pid.addWidget(self.lbl_d_standard, 3, 2)
-        self.lbl_d_parallel = QLabel("{:.4f}".format(self.kc * self.kd))
-        layout_pid.addWidget(self.lbl_d_parallel, 3, 3)
-
-        layout_pid.addWidget(QLabel("FF"), 4, 0)
-        self.slider_ff = DoubleSlider(Qt.Horizontal)
-        self.slider_ff.setMinimum(0.0)
-        self.slider_ff.setMaximum(5.0)
-        self.slider_ff.setInterval(0.01)
-        self.slider_ff.valueChanged.connect(self.updateLabelFF)
-        layout_pid.addWidget(self.slider_ff, 4, 1)
-        self.lbl_ff_standard = QLabel("{:.3f}".format(self.kff))
-        layout_pid.addWidget(self.lbl_ff_standard, 4, 2)
-        self.lbl_ff_parallel = QLabel("{:.3f}".format(self.kff))
-        layout_pid.addWidget(self.lbl_ff_parallel, 4, 3)
+        row = 1
+        for gain in self.gains.keys():
+            if gain == "FF":
+                layout_pid.addWidget(QLabel("{}".format(gain)), row, 0)
+            else:
+                layout_pid.addWidget(QLabel("K{}".format(gain.lower())), row, 0)
+
+            self.gain_slider[gain] = DoubleSlider(Qt.Horizontal)
+            self.gain_slider[gain].setMinimum(slider_props[gain]["min"])
+            self.gain_slider[gain].setMaximum(slider_props[gain]["max"])
+            self.gain_slider[gain].setInterval(slider_props[gain]["step"])
+            self.gain_slider[gain].valueChanged.connect(make_slider_callback(gain))
+            layout_pid.addWidget(self.gain_slider[gain], row, 1)
+
+            self.gain_line_edit[gain] = QLineEdit("{:.3f}".format(self.gains[gain]))
+            self.gain_line_edit[gain].setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Fixed)
+            self.gain_line_edit[gain].setMinimumWidth(0)
+            self.gain_line_edit[gain].setMinimumSize(0, 0)
+            self.gain_line_edit[gain].setAlignment(Qt.AlignCenter)
+            self.gain_line_edit[gain].textChanged.connect(make_line_edit_callback(gain))
+            layout_pid.addWidget(self.gain_line_edit[gain], row, 2)
+
+            if gain == "P" or gain == "FF":
+                self.parallel_gain_lbl[gain] = QLabel("{:.4f}".format(self.gains[gain]))
+            else:
+                self.parallel_gain_lbl[gain] = QLabel("{:.4f}".format(self.gains["P"] * self.gains[gain]))
+            self.parallel_gain_lbl[gain].setAlignment(Qt.AlignCenter)
+            layout_pid.addWidget(self.parallel_gain_lbl[gain], row, 3)
+
+            row += 1
 
         return layout_pid
 
-    def updateLabelK(self):
-        self.kc = self.slider_k.value()
-        self.lbl_k_standard.setText("{:.3f}".format(self.kc))
-        self.lbl_k_parallel.setText("{:.3f}".format(self.kc))
-
-        # Kc also modifies the Ki and Kd gains of the parallel form
-        self.lbl_i_parallel.setText("{:.2f}".format(self.kc * self.ki))
-        self.lbl_d_parallel.setText("{:.4f}".format(self.kc * self.kd))
-        if self.slider_k.isSliderDown():
-            self.updateClosedLoop()
-
-    def updateLabelI(self):
-        self.ki = self.slider_i.value()
-        self.lbl_i_standard.setText("{:.2f}".format(self.ki))
-        self.lbl_i_parallel.setText("{:.2f}".format(self.kc * self.ki))
-        if self.slider_i.isSliderDown():
+    def updateGainFromSlider(self, gain : str):
+        if self.gain_slider[gain].hasFocus():
+            self.gains[gain] = self.gain_slider[gain].value()
+            self.gain_line_edit[gain].setText("{:.3f}".format(self.gains[gain]))
+            self.updateGainLabels(gain)
+            if self.gain_slider[gain].isSliderDown():
+                self.updateClosedLoop()
+
+    def updateGainFromLineEdit(self, gain : str):
+        if isNumber(self.gain_line_edit[gain].text()) and self.gain_line_edit[gain].hasFocus():
+            self.gains[gain] = float(self.gain_line_edit[gain].text())
+            self.gain_slider[gain].setValue(self.gains[gain])
+            self.updateGainLabels(gain)
             self.updateClosedLoop()
 
-    def updateLabelD(self):
-        self.kd = self.slider_d.value()
-        self.lbl_d_standard.setText("{:.3f}".format(self.kd))
-        self.lbl_d_parallel.setText("{:.4f}".format(self.kc * self.kd))
-        if self.slider_d.isSliderDown():
-            self.updateClosedLoop()
-
-    def updateLabelFF(self):
-        self.kff = self.slider_ff.value()
-        self.lbl_ff_standard.setText("{:.3f}".format(self.kff))
-        self.lbl_ff_parallel.setText("{:.3f}".format(self.kff))
-        if self.slider_ff.isSliderDown():
-            self.updateClosedLoop()
+    def updateGainLabels(self, gain : str):
+        if gain == "FF":
+            self.parallel_gain_lbl[gain].setText("{:.4f}".format(self.gains[gain]))
+        else:
+            # Kp also modifies the Ki and Kd gains of the parallel form
+            self.parallel_gain_lbl["P"].setText("{:.4f}".format(self.gains["P"]))
+            self.parallel_gain_lbl["I"].setText("{:.4f}".format(self.gains["P"] * self.gains["I"]))
+            self.parallel_gain_lbl["D"].setText("{:.4f}".format(self.gains["P"] * self.gains["D"]))
 
     def createGmvcLayout(self):
         layout_gmvc = QFormLayout()
@@ -553,20 +553,20 @@ def computeController(self):
             sigma = self.rise_time # rise time
             delta = self.damping_index # damping property, set between 0 and 2 (1 for Butterworth)
             lbda = self.detune_coeff
-            (self.kc, self.ki, self.kd) = computePidGmvc(self.num, self.den, self.dt, sigma, delta, lbda)
+            (self.gains["P"], self.gains["I"], self.gains["D"]) = computePidGmvc(self.num, self.den, self.dt, sigma, delta, lbda)
             #TODO:find a better solution
-            self.ki /= 5.0
+            self.gains["I"] /= 5.0
             static_gain = sum(self.num) / sum(self.den)
-            self.kff = max(1 / static_gain, 0.0)
+            self.gains["FF"] = max(1 / static_gain, 0.0)
 
         self.updateKIDSliders()
         self.updateClosedLoop()
 
     def updateKIDSliders(self):
-        self.slider_k.setValue(self.kc)
-        self.slider_i.setValue(self.ki)
-        self.slider_d.setValue(self.kd)
-        self.slider_ff.setValue(self.kff)
+        for gain in self.gains.keys():
+            self.gain_line_edit[gain].setText("{:.3f}".format(self.gains[gain]))
+            self.gain_slider[gain].setValue(self.gains[gain])
+            self.updateGainLabels(gain)
 
     def updateClosedLoop(self):
         if not self.is_system_identified:
@@ -575,10 +575,10 @@ def updateClosedLoop(self):
         num = self.num
         den = self.den
         dt = self.dt
-        kc = self.kc
-        ki = self.ki
-        kd = self.kd
-        kff = self.kff
+        kc = self.gains["P"]
+        ki = self.gains["I"]
+        kd = self.gains["D"]
+        kff = self.gains["FF"]
 
         delays = ctrl.TransferFunction([1], np.append([1], np.zeros(self.sys_id_delays)), dt, inputs='r', outputs='rd')
         plant = ctrl.TransferFunction(num, den, dt, inputs='u', outputs='plant_out')