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,23 @@ def printImproperTfError(self):
         msg.exec_()
 
     def createPidLayout(self):
+        self.gain_line_edit = {
+            "P": None,
+            "I": None,
+            "D": None,
+            "FF": None
+        }
+        self.gain_slider = {
+            "P": None,
+            "I": None,
+            "D": None,
+            "FF": None
+        }
+        self.parallel_gain_lbl = {
+            "I": None,
+            "D": None,
+        }
+
         layout_pid = QGridLayout()
 
         layout_options = QHBoxLayout()
@@ -304,87 +330,106 @@ 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)
-
+        ## Proportional gain
+        layout_pid.addWidget(QLabel("Kp"), 1, 0)
+
+        self.gain_slider["P"] = DoubleSlider(Qt.Horizontal)
+        self.gain_slider["P"].setMinimum(0.001)
+        self.gain_slider["P"].setMaximum(4.0)
+        self.gain_slider["P"].setInterval(0.001)
+        self.gain_slider["P"].valueChanged.connect(lambda : self.updateGainFromSlider("P"))
+        layout_pid.addWidget(self.gain_slider["P"], 1, 1)
+
+        self.gain_line_edit["P"] = QLineEdit("{:.3f}".format(self.gains["P"]))
+        self.gain_line_edit["P"].setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Fixed)
+        self.gain_line_edit["P"].setMinimumWidth(0)
+        self.gain_line_edit["P"].setMinimumSize(0, 0)
+        self.gain_line_edit["P"].setAlignment(Qt.AlignCenter)
+        self.gain_line_edit["P"].textChanged.connect(lambda : self.updateGainFromLineEdit("P"))
+        layout_pid.addWidget(self.gain_line_edit["P"], 1, 2, 1, 2)
+
+        ## Integral gain
+        layout_pid.addWidget(QLabel("Ki"), 2, 0)
+
+        self.gain_slider["I"] = DoubleSlider(Qt.Horizontal)
+        self.gain_slider["I"].setMinimum(0.0)
+        self.gain_slider["I"].setMaximum(20.0)
+        self.gain_slider["I"].setInterval(0.1)
+        self.gain_slider["I"].valueChanged.connect(lambda : self.updateGainFromSlider("I"))
+        layout_pid.addWidget(self.gain_slider["I"], 2, 1)
+
+        self.gain_line_edit["I"] = QLineEdit("{:.3f}".format(self.gains["I"]))
+        self.gain_line_edit["I"].setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Fixed)
+        self.gain_line_edit["I"].setMinimumWidth(0)
+        self.gain_line_edit["I"].setMinimumSize(0, 0)
+        self.gain_line_edit["I"].setAlignment(Qt.AlignCenter)
+        self.gain_line_edit["I"].textChanged.connect(lambda : self.updateGainFromLineEdit("I"))
+        layout_pid.addWidget(self.gain_line_edit["I"], 2, 2)
+        self.parallel_gain_lbl["I"] = QLabel("{:.4f}".format(self.gains["P"] * self.gains["I"]))
+        self.parallel_gain_lbl["I"].setAlignment(Qt.AlignCenter)
+        layout_pid.addWidget(self.parallel_gain_lbl["I"], 2, 3)
+
+        ## Derivative gain
+        layout_pid.addWidget(QLabel("Kd"), 3, 0)
+
+        self.gain_slider["D"] = DoubleSlider(Qt.Horizontal)
+        self.gain_slider["D"].setMinimum(0.0)
+        self.gain_slider["D"].setMaximum(0.2)
+        self.gain_slider["D"].setInterval(0.001)
+        self.gain_slider["D"].valueChanged.connect(lambda : self.updateGainFromSlider("D"))
+        layout_pid.addWidget(self.gain_slider["D"], 3, 1)
+
+        self.gain_line_edit["D"] = QLineEdit("{:.3f}".format(self.gains["D"]))
+        self.gain_line_edit["D"].setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Fixed)
+        self.gain_line_edit["D"].setMinimumWidth(0)
+        self.gain_line_edit["D"].setMinimumSize(0, 0)
+        self.gain_line_edit["D"].setAlignment(Qt.AlignCenter)
+        self.gain_line_edit["D"].textChanged.connect(lambda : self.updateGainFromLineEdit("D"))
+        layout_pid.addWidget(self.gain_line_edit["D"], 3, 2)
+        self.parallel_gain_lbl["D"] = QLabel("{:.4f}".format(self.gains["P"] * self.gains["D"]))
+        self.parallel_gain_lbl["D"].setAlignment(Qt.AlignCenter)
+        layout_pid.addWidget(self.parallel_gain_lbl["D"], 3, 3)
+
+        ## Feedforward gain
         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)
-
-        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))
+        self.gain_slider["FF"] = DoubleSlider(Qt.Horizontal)
+        self.gain_slider["FF"].setMinimum(0.0)
+        self.gain_slider["FF"].setMaximum(5.0)
+        self.gain_slider["FF"].setInterval(0.01)
+        self.gain_slider["FF"].valueChanged.connect(lambda : self.updateGainFromSlider("FF"))
+        layout_pid.addWidget(self.gain_slider["FF"], 4, 1)
+
+        self.gain_line_edit["FF"] = QLineEdit("{:.3f}".format(self.gains["FF"]))
+        self.gain_line_edit["FF"].setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Fixed)
+        self.gain_line_edit["FF"].setMinimumWidth(0)
+        self.gain_line_edit["FF"].setMinimumSize(0, 0)
+        self.gain_line_edit["FF"].setAlignment(Qt.AlignCenter)
+        self.gain_line_edit["FF"].textChanged.connect(lambda : self.updateGainFromLineEdit("FF"))
+        layout_pid.addWidget(self.gain_line_edit["FF"], 4, 2, 1, 2)
 
-        # 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():
-            self.updateClosedLoop()
+        return layout_pid
 
-    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():
+    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 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":
+            # Kp also modifies the Ki and Kd gains of the parallel form
+            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 +598,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 +620,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')