PythonとPyQtGraphで台形駆動のグラフを描いてみる

Share on:

機械設計をしているとモーターを使っている部分の仕様を描く場合に度々台形駆動のグラフを描くことがあります。今まではエクセルで点をたくさんプロットしてグラフを描いていましたが、試しにPythonで書いてみることにしました。

台形駆動のプロット例

GUIの上部に数値を入力してShow graphをクリックすると、入力された値から計算した値がプロットされます。

  • acceleration : 加速度
  • v_max : 最高速度
  • distance : 移動距離
  • step : 横軸1区間のプロット数(10としたら加速区間で10点プロットする)
  • decimals : 有効数字

三角駆動のプロット例

移動距離distanceの半分の距離を等加速度運動したときの速度がv_maxを超える場合は台形駆動、等しいかそれよりも小さい場合は三角駆動として計算するように場合分けしています。

エラー例

GUI下部のテキストブラウザにエラーが表示されます。例は加速度が0なので、0で割る計算ができないエラーです。

ソースコード

main.pyとmotor_graph.pyを同じフォルダに入れてmain.pyを実行するとGUIが表示されます。実行にはPyQt5、PyQtGraph、Numpyが必要です。

main.py

 1# -*- coding: utf-8 -*-
 2import sys
 3from PyQt5 import QtWidgets, QtCore
 4import pyqtgraph as pg
 5
 6from motor_graph import motor_graph
 7
 8class MainWindow(QtWidgets.QMainWindow):
 9    def __init__(self, parent=None):
10        super(MainWindow, self).__init__(parent)
11        
12        # resize window size
13        self.resize(600,800)
14        self.setStyleSheet("QMainWindow {background: 'white';}")
15
16        # create widgets and layouts
17        self.centralwidget = QtWidgets.QWidget()
18        self.centralLayout = QtWidgets.QVBoxLayout(self.centralwidget)
19        self.setCentralWidget(self.centralwidget)
20
21        # create line edit and button
22        self.horizontal_widget = QtWidgets.QWidget()
23        self.horizontalLayout = QtWidgets.QHBoxLayout(self.horizontal_widget)
24        self.line_edit, self.label = {}, {}
25        contents = ['acceleration[m/ss]', 'v_max[m/s]', 'distance[m]', 'step', 'decimals']
26        for content in contents:
27            self.label[content] = QtWidgets.QLabel(content)
28            self.line_edit[content] = QtWidgets.QLineEdit()
29            self.horizontalLayout.addWidget(self.label[content])
30            self.horizontalLayout.addWidget(self.line_edit[content])
31        self.line_edit['acceleration[m/ss]'].setText('10.0')
32        self.line_edit['v_max[m/s]'].setText('1.0')
33        self.line_edit['distance[m]'].setText('0.3')
34        self.line_edit['step'].setText('10')
35        self.line_edit['decimals'].setText('8')
36        self.button = QtWidgets.QPushButton('Show graph')
37        self.button.clicked.connect(self.motor_graph)
38        self.horizontalLayout.addWidget(self.button)
39        self.centralLayout.addWidget(self.horizontal_widget)
40
41        # create plot widgets
42        self.graph_widget_ta = self.plot_widget('t [sec]', 'a [m/ss]')
43        self.graph_widget_tv = self.plot_widget('t [sec]', 'v [m/s]')
44        self.graph_widget_tx = self.plot_widget('t [sec]', 'x [m]')
45
46        # set plot widgets
47        self.vertical_widget = QtWidgets.QWidget()
48        self.verticalLayout = QtWidgets.QVBoxLayout(self.vertical_widget)
49        self.verticalLayout.addWidget(self.graph_widget_ta)
50        self.verticalLayout.addWidget(self.graph_widget_tv)
51        self.verticalLayout.addWidget(self.graph_widget_tx)
52        self.centralLayout.addWidget(self.vertical_widget)
53
54        # text blowser
55        self.text_browser = QtWidgets.QTextBrowser()
56        self.verticalLayout.addWidget(self.text_browser)
57
58    def motor_graph(self):
59        try:
60            self.graph_widget_ta.clear()
61            self.graph_widget_tv.clear()
62            self.graph_widget_tx.clear()
63            acceleration = float( self.line_edit['acceleration[m/ss]'].text() )
64            v_max = float( self.line_edit['v_max[m/s]'].text() )
65            distance = float( self.line_edit['distance[m]'].text() )
66            step = int( self.line_edit['step'].text() )
67            decimals = int( self.line_edit['decimals'].text() )
68            t, a, v, x = motor_graph(acceleration, v_max, distance, step, decimals)
69            pen = pg.mkPen(color='#000000', width=1)
70            for ti, ai, vi, xi in zip(t, a, v, x):
71                self.graph_widget_ta.addItem( pg.PlotDataItem(x=ti, y=ai, pen=pen, antialias=True) )
72                self.graph_widget_tv.addItem( pg.PlotDataItem(x=ti, y=vi, pen=pen, antialias=True) )
73                self.graph_widget_tx.addItem( pg.PlotDataItem(x=ti, y=xi, pen=pen, antialias=True) )
74        except:
75            self.text_browser.setText('Error!!!\n\n' + '\n'.join( [str(i) for i in sys.exc_info()] ))
76
77    def plot_widget(self, label_x, label_y):
78        view_box = pg.ViewBox(border=pg.mkPen(color='#000000'))
79        pw = pg.PlotWidget(parent=self, background='#FFFFFFFF', viewBox=view_box)
80
81        pi = pw.plotItem
82        # set axis label text
83        pi.setLabels(bottom=label_x, left = label_y)
84        # set x axis color to black
85        pi.getAxis('bottom').setPen(pg.mkPen(color='#000000'))
86        # set y axis color to black
87        pi.getAxis('left').setPen(pg.mkPen(color='#000000'))
88        return pw
89
90def main():
91    app = QtWidgets.QApplication(sys.argv)
92    window = MainWindow()
93    window.show()
94    app.exec()
95
96if __name__ == '__main__':
97    main()

motor_graph.py

 1# -*- coding: utf-8 -*-
 2import numpy as np
 3
 4def uniformly_accelerated_motion(v0, t0, t1, x0, acceleration, steps, decimals=4):
 5    t = np.round( np.linspace(t0, t1, steps), decimals)
 6    t_tmp = np.round( np.linspace(0, t1-t0, steps), decimals)
 7    a = np.round( np.full(steps, acceleration), decimals)
 8    v = np.round( v0 + a * t_tmp, decimals)
 9    # x = v0 t + 1/2 a t^2
10    x = np.round( v0 * t_tmp + 0.5 * a * t_tmp ** 2 + x0, decimals)
11    return t, a, v, x
12
13def uniform_linear_motion(v0, t0, t1, x0, steps, decimals=4):
14    t = np.round( np.linspace(t0, t1, steps), decimals)
15    t_tmp = np.round( np.linspace(0, t1-t0, steps), decimals)
16    a = np.round( np.zeros(steps), decimals)
17    v = np.round( np.full(steps, v0), decimals)
18    x = np.round( v0 * t_tmp + x0, decimals)
19    return t, a, v, x
20
21def motor_graph(acceleration, v_max, distance, step, decimals=4, v0=0):
22
23    # v**2 - v0**2 = 2as   ->   v**2 = 2as - v0**2   ->   v = (2as - v0**2) ** 0.5
24    v_tmp = (2.0*acceleration*(distance / 2.0) - v0**2.0) ** 0.5
25
26    if v_tmp <= v_max:
27        # 三角駆動
28        #v_max2 = round( (v0 ** 2 + 2 * acceleration * distance / 2.0) ** 0.5, 6 )
29        t_tmp = round( (v_tmp - v0) / acceleration, decimals )
30        
31        # 加速
32        t1, a1, v1, x1 = uniformly_accelerated_motion(0.0, 0.0, t_tmp, 0.0, acceleration, step, decimals)
33        
34        # 減速
35        #t2, a2, v2, x2 = uniformly_accelerated_motion(v_max2, t1[-1], t1[-1] + t1[-1], x1[-1], -1.0*acceleration, step, decimals)
36        t2, a2, v2, x2 = uniformly_accelerated_motion(v_tmp, t1[-1], t1[-1] + t1[-1], x1[-1], -1.0*acceleration, step, decimals)
37
38        t = [ t1, t2 ]
39        a = [ a1, a2 ]
40        v = [ v1, v2 ]
41        x = [ x1, x2 ]
42
43    else:
44        # 台形駆動
45        #t_tmp = (v_max - v0) / acceleration
46        t_tmp = round( (v_max - v0) / acceleration, decimals )
47
48        # 加速
49        t1, a1, v1, x1 = uniformly_accelerated_motion(
50            v0=0.0, t0=0.0, t1=t_tmp, x0=0.0, acceleration=acceleration, steps=step
51        )
52        
53        # 等速
54        # x=at   ->   t=x/a   ->   (distance - x1[-1] * 2) / v_max
55        t1_tmp = round( (distance - x1[-1] * 2) / v_max + t1[-1], decimals )
56        t2, a2, v2, x2 = uniform_linear_motion(v0=v_max, t0=t1[-1], t1=t1_tmp, x0=x1[-1], steps=step)
57
58        # 減速
59        t3, a3, v3, x3 = uniformly_accelerated_motion(v_max, t2[-1], t2[-1] + t1[-1], x2[-1], -1.0*acceleration, step)
60
61        t = [ t1, t2, t3 ]
62        a = [ a1, a2, a3 ]
63        v = [ v1, v2, v3 ]
64        x = [ x1, x2, x3 ]
65    
66    return t, a, v, x
67
68def main():
69    t, a, v, x = motor_graph(10.0, 1.0, 0.35, 10)
70    
71    print('t', t)
72    print('a', a)
73    print('v', v)
74    print('x', x)
75
76if __name__ == '__main__':
77    main()

関連記事