diff --git a/autotune/README.md b/autotune/README.md index 9aa72aa..9dbca96 100644 --- a/autotune/README.md +++ b/autotune/README.md @@ -27,7 +27,7 @@ pip3 install numpy scipy pyulog control pyqt5 python3 autotune.py ``` -![image](https://github.com/user-attachments/assets/fcdf5c25-d92d-4487-9736-e77f6576d180) +![Autotune GUI main window](docs/gui_main_window.png) # Tuning Worflow diff --git a/autotune/autotune.py b/autotune/autotune.py index 99f10a4..188a8ff 100644 --- a/autotune/autotune.py +++ b/autotune/autotune.py @@ -61,6 +61,7 @@ QHeaderView, QLabel, QLineEdit, + QMenu, QMessageBox, QPushButton, QRadioButton, @@ -72,6 +73,7 @@ QTabWidget, QVBoxLayout, QWidget, + QWidgetAction, ) from scipy.signal import detrend from system_identification import SystemIdentification @@ -168,6 +170,14 @@ def run(self): self.finished.emit(best_params, best_fit) +def thresholdColor(value, limit, limit_is_minimum): + """Green when `value` respects `limit`, red when it violates it, None if unknown.""" + if value is None or not np.isfinite(value): + return None + respected = value >= limit if limit_is_minimum else value <= limit + return "green" if respected else "red" + + def isNumber(value): try: float(value) @@ -183,6 +193,7 @@ def __init__(self, parent=None): self.model_ref = None self.input_ref = None self.closed_loop_ref = None + self.closed_loop_step_ref = None self.closed_loop_ax = None self.measured_step_info = None self.step_info_patches = [] @@ -194,6 +205,7 @@ def __init__(self, parent=None): "settling_time": 0.4, } self.bode_plot_ref = [] + self.margin_text_refs = {} self.pz_plot_refs = [] self.file_name = None self.is_system_identified = False @@ -212,6 +224,11 @@ def __init__(self, parent=None): self.sys_id_n_poles = 2 self.kDisturbanceTime = 1.0 + self.kMinGainMarginDb = 6.0 + self.kMinPhaseMarginDeg = 45.0 + self.step_duration = 2.0 + self.disturbance_amplitude = -0.05 + self.step_sim_spinbox = {} # this is the Canvas Widget that displays the `figure` # it takes the `figure` instance as a parameter to __init__ @@ -281,6 +298,7 @@ def reset(self): self.model_ref = None self.input_ref = None self.closed_loop_ref = None + self.closed_loop_step_ref = None self.measured_step_info = None self.step_info_patches = [] self.lbl_fit.setText("—") @@ -289,6 +307,7 @@ def reset(self): self.lbl_stability.setStyleSheet("") self.btn_stabilize.setVisible(False) self.bode_plot_ref = [] + self.margin_text_refs = {} self.pz_plot_refs = [] self.is_system_identified = False @@ -324,7 +343,7 @@ def createPreprocessingWidget(self, layout): self.f_hp_spinbox.setRange(0.0, 50.0) self.f_hp_spinbox.setSingleStep(0.1) self.f_hp_spinbox.setDecimals(1) - self.f_hp_spinbox.setValue(0.5) + self.f_hp_spinbox.setValue(0.0) self.f_hp_spinbox.valueChanged.connect( lambda: self.btn_run_sys_id.setEnabled(True) ) @@ -628,9 +647,71 @@ def createStepInfoGroup(self): grid.addWidget(sb, row, 1) grid.addWidget(measured_lbl, row, 2) + btn_plot_options = QPushButton("Plot options") + btn_plot_options.setMenu(self.createPlotOptionsMenu(btn_plot_options)) + grid.addWidget(btn_plot_options, len(specs) + 1, 0, 1, 3) + group.setLayout(grid) return group + def createPlotOptionsMenu(self, parent): + sim_specs = { + "step_duration": ( + "Plot duration", + self.step_duration, + 0.1, + 20.0, + 1.0, + 1, + "s", + ), + "disturbance_time": ( + "Disturbance time", + self.kDisturbanceTime, + 0.0, + 20.0, + 1.0, + 1, + "s", + ), + "disturbance_amplitude": ( + "Disturbance ampl.", + self.disturbance_amplitude, + -1.0, + 1.0, + 0.1, + 2, + "", + ), + } + menu = QMenu(parent) + content = QWidget(menu) + form = QFormLayout(content) + for key, (label, default, lo, hi, step, decimals, unit) in sim_specs.items(): + sb = QDoubleSpinBox() + sb.setRange(lo, hi) + sb.setSingleStep(step) + sb.setDecimals(decimals) + sb.setValue(default) + sb.valueChanged.connect(self.onStepSimChanged) + self.step_sim_spinbox[key] = sb + + unit_str = " (" + unit + ")" if unit else "" + form.addRow(label + unit_str, sb) + + action = QWidgetAction(menu) + action.setDefaultWidget(content) + menu.addAction(action) + return menu + + def onStepSimChanged(self): + self.step_duration = self.step_sim_spinbox["step_duration"].value() + self.kDisturbanceTime = self.step_sim_spinbox["disturbance_time"].value() + self.disturbance_amplitude = self.step_sim_spinbox[ + "disturbance_amplitude" + ].value() + self.updateClosedLoop() + def onStepInfoChanged(self): for key in self.step_info: self.step_info[key] = self.step_info_spinbox[key].value() @@ -670,12 +751,10 @@ def updateStepInfoEnvelope(self): else: measured = self.measured_step_info[info_key] lbl.setText(fmt(measured)) - if np.isnan(measured): - lbl.setStyleSheet("") - elif measured > self.step_info[key]: - lbl.setStyleSheet("color: red") - else: - lbl.setStyleSheet("color: green") + color = thresholdColor( + measured, self.step_info[key], limit_is_minimum=False + ) + lbl.setStyleSheet(f"color: {color}" if color else "") self.canvas.draw() @@ -1037,7 +1116,9 @@ def updateClosedLoop(self): outputs="y", ) - t_out, y_out = ctrl.step_response(closed_loop, T=np.arange(0, 2, dt)) + t_out, y_out = ctrl.step_response( + closed_loop, T=np.arange(0, self.step_duration, dt) + ) # Add disturbance sum_feedback_no_ref = ctrl.summing_junction(inputs=["-y"], output="e") @@ -1060,7 +1141,7 @@ def updateClosedLoop(self): outputs="y", ) d = np.zeros_like(t_out) - d[t_out >= self.kDisturbanceTime] = -0.05 # TODO: parameterize + d[t_out >= self.kDisturbanceTime] = self.disturbance_amplitude _, y_d = ctrl.forced_response(disturbance_loop, t_out, d) y_out += y_d @@ -1101,9 +1182,10 @@ def plotClosedLoop(self, t, y): except (IndexError, ValueError): self.measured_step_info = None + step_ref = [1 if i > 0 else 0 for i in t] if self.closed_loop_ref is None: ax = self.figure.add_subplot(3, 3, 7) - ax.step(t, [1 if i > 0 else 0 for i in t], "k--") + self.closed_loop_step_ref = ax.step(t, step_ref, "k--")[0] plot_ref = ax.plot(t, y) self.closed_loop_ref = plot_ref[0] self.closed_loop_ax = ax @@ -1111,9 +1193,11 @@ def plotClosedLoop(self, t, y): ax.set_xlabel("Time (s)") ax.set_ylabel("Amplitude (rad/s)") else: + self.closed_loop_step_ref.set_data(t, step_ref) self.closed_loop_ref.set_xdata(t) self.closed_loop_ref.set_ydata(y) self.closed_loop_ax.set_ylim(np.min(y), np.max([1.5, np.max(y)])) + self.closed_loop_ax.set_xlim(t[0], t[-1]) self.updateStepInfoEnvelope() @@ -1127,7 +1211,21 @@ def plotBode(self, open_loop, closed_loop): gain_crossover, stab_margin_w, ) = ctrl.stability_margins(open_loop) - stability_margins_text = f"Gain margin: {20 * np.log10(gain_margin):.2f}dB (@{phase_crossover / (2 * np.pi):.1f}Hz)\nPhase margin: {phase_margin:.1f}deg (@{gain_crossover / (2 * np.pi):.1f}Hz)" + gain_margin_db = 20 * np.log10(gain_margin) + margins = { + "gain": ( + f"Gain margin: {gain_margin_db:.2f}dB (@{phase_crossover / (2 * np.pi):.1f}Hz)", + thresholdColor( + gain_margin_db, self.kMinGainMarginDb, limit_is_minimum=True + ), + ), + "phase": ( + f"Phase margin: {phase_margin:.1f}deg (@{gain_crossover / (2 * np.pi):.1f}Hz)", + thresholdColor( + phase_margin, self.kMinPhaseMarginDeg, limit_is_minimum=True + ), + ), + } w = np.logspace(-1, 3, 40).tolist() (mag_ol, phase_ol, omega_ol) = ctrl.frequency_response( @@ -1162,13 +1260,14 @@ def plotBode(self, open_loop, closed_loop): ax.set_xlabel("Frequency (Hz)") ax.set_ylabel("Phase (deg)") - self.gain_margin_text_ref = ax.text( - 0.01, - 0.9, - stability_margins_text, - verticalalignment="top", - transform=ax.transAxes, - ) + for row, key in enumerate(margins): + self.margin_text_refs[key] = ax.text( + 0.01, + 0.9 - 0.12 * row, + "", + verticalalignment="top", + transform=ax.transAxes, + ) else: self.bode_plot_ref[0].set_xdata(f) @@ -1177,13 +1276,16 @@ def plotBode(self, open_loop, closed_loop): self.bode_plot_ref[1].set_xdata(f) mag_cl_db = 20 * np.log10(mag_cl) self.bode_plot_ref[1].set_ydata(mag_cl_db) - self.gain_margin_text_ref.set_text(stability_margins_text) self.bode_plot_ref[2].set_xdata(f) self.bode_plot_ref[2].set_ydata(phase_ol * 180 / np.pi) self.bode_plot_ref[3].set_xdata(f) self.bode_plot_ref[3].set_ydata(phase_cl * 180 / np.pi) + for key, (text, color) in margins.items(): + self.margin_text_refs[key].set_text(text) + self.margin_text_refs[key].set_color(color or "black") + self.canvas.draw() def plotInputOutput(self, redraw=False): diff --git a/autotune/docs/gui_main_window.png b/autotune/docs/gui_main_window.png new file mode 100644 index 0000000..62733c7 Binary files /dev/null and b/autotune/docs/gui_main_window.png differ