-
Notifications
You must be signed in to change notification settings - Fork 6
/
rc_motor.py
executable file
·161 lines (120 loc) · 3.73 KB
/
rc_motor.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
#!/usr/bin/env python3
def main():
# import python's standard math module and numpy
import math, numpy, sys
# import Controller and other blocks from modules
from pyctrl.rc import Controller
from pyctrl.block import Interp, Logger, Constant
from pyctrl.block.system import System, Differentiator
from pyctrl.system.tf import DTTF, LPF
# initialize controller
Ts = 0.01
bbb = Controller(period = Ts)
# add encoder as source
bbb.add_source('encoder1',
('pyctrl.rc.encoder', 'Encoder'),
['encoder'],
kwargs = {'encoder': 3,
'ratio': 60 * 35.557})
# add motor as sink
bbb.add_sink('motor1',
('pyctrl.rc.motor', 'Motor'),
['pwm'],
kwargs = {'motor': 3},
enable = True)
# build interpolated input signal
ts = [0, 1, 2, 3, 4, 5, 5, 6]
us = [0, 0, 100, 100, -50, -50, 0, 0]
# add filter to interpolate data
bbb.add_filter('input',
Interp(xp = us, fp = ts),
['clock'],
['pwm'])
# add motor speed signal
bbb.add_signal('speed')
# add motor speed filter
bbb.add_filter('speed',
Differentiator(),
['clock','encoder'],
['speed'])
# add low-pass signal
bbb.add_signal('fspeed')
# add low-pass filter
bbb.add_filter('LPF',
System(model = LPF(fc = 5, period = Ts)),
['speed'],
['fspeed'])
# add logger
bbb.add_sink('logger',
Logger(),
['clock','pwm','encoder','speed','fspeed'])
# Add a timer to stop the controller
bbb.add_timer('stop',
Constant(value = 0),
None, ['is_running'],
period = 6, repeat = False)
# print controller info
print(bbb.info('all'))
try:
# run the controller
print('> Run the controller.')
# reset clock
bbb.set_source('clock', reset = True)
with bbb:
# wait for the controller to finish on its own
bbb.join()
print('> Done with the controller.')
except KeyboardInterrupt:
pass
# read logger
data = bbb.get_sink('logger', 'log')
try:
# import matplotlib
import matplotlib.pyplot as plt
except:
print('! Could not load matplotlib, skipping plots')
sys.exit(0)
print('> Will plot')
try:
# start plot
plt.figure()
except:
print('! Could not plot graphics')
print('> Make sure you have a connection to a windows manager')
sys.exit(0)
# plot pwm
plt.subplot(2,1,1)
plt.plot(data['clock'], data['pwm'], 'b')
plt.ylabel('pwm (%)')
plt.ylim((-120,120))
plt.xlim(0,6)
plt.grid()
# plot encoder
plt.subplot(2,1,2)
plt.plot(data['clock'], data['encoder'],'b')
plt.ylabel('encoder (cycles)')
plt.ylim((0,25))
plt.xlim(0,6)
plt.grid()
# start plot
plt.figure()
# plot pwm
ax1 = plt.gca()
ax1.plot(data['clock'], data['pwm'],'g', label='pwm')
ax1.set_ylabel('pwm (%)')
ax1.set_ylim((-60,120))
ax1.grid()
plt.legend(loc = 2)
# plot velocity
ax2 = plt.twinx()
ax2.plot(data['clock'], data['speed'],'b', label='speed')
ax2.plot(data['clock'], data['fspeed'], 'r', label='fspeed')
ax2.set_ylabel('speed (Hz)')
ax2.set_ylim((-6,12))
ax2.set_xlim(0,6)
ax2.grid()
plt.legend(loc = 1)
# show plots
plt.show()
if __name__ == "__main__":
main()