def draw_path(self, gc, path, transform, rgbFace=None):
path = transform.transform_path(path)
points = path.vertices
codes = path.codes
bbox = gc.get_clip_rectangle()
if bbox is not None:
x, y, w, h = bbox.bounds
clrt = np.array([x, x + w, y, y + h])
else:
clrt = np.array([0, self.width, 0, self.height])
gr.setviewport(*clrt / self.size)
gr.setwindow(*clrt)
if rgbFace is not None and len(points) > 2:
color = gr.inqcolorfromrgb(rgbFace[0], rgbFace[1], rgbFace[2])
gr.settransparency(rgbFace[3])
gr.setcolorrep(color, rgbFace[0], rgbFace[1], rgbFace[2])
gr.setfillintstyle(gr.INTSTYLE_SOLID)
gr.setfillcolorind(color)
gr.drawpath(points, codes, fill=True)
lw = gc.get_linewidth()
if lw != 0:
rgba = gc.get_rgb()[:4]
color = gr.inqcolorfromrgb(rgba[0], rgba[1], rgba[2])
gr.settransparency(rgba[3])
gr.setcolorrep(color, rgba[0], rgba[1], rgba[2])
if isinstance(gc._linestyle, str):
gr.setlinetype(linetype[gc._linestyle])
gr.setlinewidth(lw)
gr.setlinecolorind(color)
gr.drawpath(points, codes, fill=False)
def _draw_legend():
global _plt
viewport = _plt.kwargs['viewport']
num_labels = len(_plt.kwargs['labels'])
location = _plt.kwargs.get('location', 1)
gr.savestate()
gr.selntran(0)
gr.setscale(0)
w = 0
for label in _plt.kwargs['labels']:
tbx, tby = gr.inqtextext(0, 0, label)
w = max(w, tbx[2])
num_lines = len(_plt.args)
h = (num_lines + 1) * 0.03
if location in (8, 9, 10):
px = 0.5 * (viewport[0] + viewport[1] - w)
elif location in (2, 3, 6):
px = viewport[0] + 0.11
else:
px = viewport[1] - 0.05 - w
if location in (5, 6, 7, 10):
py = 0.5 * (viewport[2] + viewport[3] + h) - 0.03
elif location in (3, 4, 8):
py = viewport[2] + h
else:
py = viewport[3] - 0.06
gr.setfillintstyle(gr.INTSTYLE_SOLID)
gr.setfillcolorind(0)
gr.fillrect(px - 0.08, px + w + 0.02, py + 0.03, py - 0.03 * num_lines)
gr.setlinetype(gr.LINETYPE_SOLID)
gr.setlinecolorind(1)
gr.setlinewidth(1)
gr.drawrect(px - 0.08, px + w + 0.02, py + 0.03, py - 0.03 * num_lines)
i = 0
gr.uselinespec(" ")
for (x, y, z, c, spec) in _plt.args:
gr.savestate()
mask = gr.uselinespec(spec)
if mask in (0, 1, 3, 4, 5):
gr.polyline([px - 0.07, px - 0.01], [py, py])
if mask & 2:
gr.polymarker([px - 0.06, px - 0.02], [py, py])
gr.restorestate()
gr.settextalign(gr.TEXT_HALIGN_LEFT, gr.TEXT_VALIGN_HALF)
if i < num_labels:
gr.textext(px, py, _plt.kwargs['labels'][i])
i += 1
py -= 0.03
gr.selntran(1)
gr.restorestate()
def _draw_axes(kind, pass_=1):
global _plt
viewport = _plt.kwargs['viewport']
vp = _plt.kwargs['vp']
x_tick, x_org, x_major_count = _plt.kwargs['xaxis']
y_tick, y_org, y_major_count = _plt.kwargs['yaxis']
gr.setlinecolorind(1)
gr.setlinewidth(1)
diag = ((viewport[1]-viewport[0])**2 + (viewport[3]-viewport[2])**2)**0.5
charheight = max(0.018 * diag, 0.012)
gr.setcharheight(charheight)
ticksize = 0.0075 * diag
if kind in ('wireframe', 'surface', 'plot3', 'scatter3', 'trisurf'):
z_tick, z_org, z_major_count = _plt.kwargs['zaxis']
if pass_ == 1:
gr.grid3d(x_tick, 0, z_tick, x_org[0], y_org[0], z_org[0], 2, 0, 2)
gr.grid3d(0, y_tick, 0, x_org[1], y_org[0], z_org[0], 0, 2, 0)
else:
gr.axes3d(x_tick, 0, z_tick, x_org[0], y_org[0], z_org[0], x_major_count, 0, z_major_count, -ticksize)
gr.axes3d(0, y_tick, 0, x_org[1], y_org[0], z_org[0], 0, y_major_count, 0, ticksize)
else:
if kind == 'heatmap':
ticksize = -ticksize
else:
gr.grid(x_tick, y_tick, 0, 0, x_major_count, y_major_count)
gr.axes(x_tick, y_tick, x_org[0], y_org[0], x_major_count, y_major_count, ticksize)
gr.axes(x_tick, y_tick, x_org[1], y_org[1], -x_major_count, -y_major_count, -ticksize)
if 'title' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_TOP)
gr.textext(0.5*(viewport[0] + viewport[1]), vp[3], _plt.kwargs['title'])
gr.restorestate()
if kind in ('wireframe', 'surface', 'plot3', 'scatter3', 'trisurf'):
x_label = _plt.kwargs.get('xlabel', '')
y_label = _plt.kwargs.get('ylabel', '')
z_label = _plt.kwargs.get('zlabel', '')
gr.titles3d(x_label, y_label, z_label)
else:
if 'xlabel' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_BOTTOM)
gr.textext(0.5 * (viewport[0] + viewport[1]), vp[2] + 0.5 * charheight, _plt.kwargs['xlabel'])
gr.restorestate()
if 'ylabel' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_TOP)
gr.setcharup(-1, 0)
gr.textext(vp[0] + 0.5 * charheight, 0.5 * (viewport[2] + viewport[3]), _plt.kwargs['ylabel'])
gr.restorestate()
def drawGR(self):
if self.linetype is not None and self.x:
# preserve old values
lcolor = gr.inqlinecolorind()
gr.setlinewidth(2)
gr.setlinecolorind(self.linecolor)
for xi, yi in zip(self.x, self.y):
gr.polyline([xi, xi], [0, yi])
# restore old values
gr.setlinecolorind(lcolor)
gr.setlinewidth(1)
def drawGR(self):
lwidth = gr.inqlinewidth()
gr.setlinewidth(0.)
PlotAxes.drawGR(self)
if self.drawxylines:
xmin, xmax, ymin, ymax = self.getWindow()
linecolor = gr.inqlinecolorind()
gr.setlinecolorind(self.xylinecolor)
for xpos in self.xlines:
gr.polyline([xpos, xpos], [ymin, ymax])
for ypos in self.ylines:
gr.polyline([xmin, xmax], [ypos, ypos])
gr.setlinecolorind(linecolor)
gr.setlinewidth(lwidth)
def pendulum(t, theta, omega, acceleration):
gr.clearws()
gr.setviewport(0, 1, 0, 1)
x = [0.5, 0.5 + np.sin(theta) * 0.4]
y = [0.8, 0.8 - np.cos(theta) * 0.4]
# draw pivot point
gr.fillarea([0.46, 0.54, 0.54, 0.46], [0.79, 0.79, 0.81, 0.81]),
gr.setlinecolorind(1)
gr.setlinewidth(2)
gr.polyline(x, y) # draw rod
gr.setmarkersize(5)
gr.setmarkertype(gr.MARKERTYPE_SOLID_CIRCLE)
gr.setmarkercolorind(86)
gr.polymarker([x[1]], [y[1]]) # draw bob
gr.setlinecolorind(4)
V = 0.05 * omega # show angular velocity
gr.drawarrow(x[1], y[1], x[1] + V * np.cos(theta),
y[1] + V * np.sin(theta))
gr.setlinecolorind(2)
A = 0.05 * acceleration # show angular acceleration
gr.drawarrow(x[1], y[1], x[1] + A * np.sin(theta),
y[1] + A * np.cos(theta))
gr.settextfontprec(2, gr.TEXT_PRECISION_STRING)
gr.setcharheight(0.032)
gr.settextcolorind(1)
gr.textext(0.05, 0.95, 'Damped Pendulum')
gr.setcharheight(0.040)
gr.mathtex(0.4, 0.22, '\\omega=\\dot{\\theta}')
gr.mathtex(0.4, 0.1,
'\\dot{\\omega}=-\\gamma\\omega-\\frac{g}{l}sin(\\theta)')
gr.setcharheight(0.028)
gr.textext(0.05, 0.22, 't:%7.2f' % t)
gr.textext(0.05, 0.16, '\\theta:%7.2f' % (theta / np.pi * 180))
gr.settextcolorind(4)
gr.textext(0.05, 0.10, '\\omega:%7.2f' % omega)
gr.settextcolorind(2)
gr.textext(0.05, 0.04, 'y_{A}:%6.2f' % acceleration)
gr.updatews()
def pendulum(t, theta, omega, acceleration):
gr.clearws()
gr.setviewport(0, 1, 0, 1)
x = [0.5, 0.5 + sin(theta) * 0.4]
y = [0.8, 0.8 - cos(theta) * 0.4]
# draw pivot point
gr.fillarea([0.46, 0.54, 0.54, 0.46], [0.79, 0.79, 0.81, 0.81]),
gr.setlinecolorind(1)
gr.setlinewidth(2)
gr.polyline(x, y) # draw rod
gr.setmarkersize(5)
gr.setmarkertype(gr.MARKERTYPE_SOLID_CIRCLE)
gr.setmarkercolorind(86)
gr.polymarker([x[1]], [y[1]]) # draw bob
gr.setlinecolorind(4)
V = 0.05 * omega # show angular velocity
gr.drawarrow(x[1], y[1], x[1] + V*cos(theta), y[1] + V*sin(theta))
gr.setlinecolorind(2)
A = 0.05 * acceleration # show angular acceleration
gr.drawarrow(x[1], y[1], x[1] + A*sin(theta), y[1] + A*cos(theta))
gr.settextfontprec(2, gr.TEXT_PRECISION_STRING)
gr.setcharheight(0.032)
gr.settextcolorind(1)
gr.textext(0.05, 0.95, 'Damped Pendulum')
gr.setcharheight(0.040)
gr.mathtex(0.4, 0.22, '\\omega=\\dot{\\theta}')
gr.mathtex(0.4, 0.1, '\\dot{\\omega}=-\\gamma\\omega-\\frac{g}{l}sin(\\theta)')
gr.setcharheight(0.028)
gr.textext(0.05, 0.22, 't:%7.2f' % t)
gr.textext(0.05, 0.16, '\\theta:%7.2f' % (theta / pi * 180))
gr.settextcolorind(4)
gr.textext(0.05, 0.10, '\\omega:%7.2f' % omega)
gr.settextcolorind(2)
gr.textext(0.05, 0.04, 'y_{A}:%6.2f' % acceleration)
gr.updatews()
def _draw_legend():
global _plt
viewport = _plt.kwargs['viewport']
num_labels = len(_plt.kwargs['labels'])
gr.savestate()
gr.selntran(0)
gr.setscale(0)
w = 0
for label in _plt.kwargs['labels']:
tbx, tby = gr.inqtextext(0, 0, label)
w = max(w, tbx[2])
px = viewport[1] - 0.05 - w
py = viewport[3] - 0.06
gr.setfillintstyle(gr.INTSTYLE_SOLID)
gr.setfillcolorind(0)
gr.fillrect(px - 0.08, px + w + 0.02, py + 0.03, py - 0.03 * num_labels)
gr.setlinetype(1)
gr.setlinecolorind(1)
gr.setlinewidth(1)
gr.drawrect(px - 0.08, px + w + 0.02, py + 0.03, py - 0.03 * num_labels)
i = 0
gr.uselinespec(" ")
for (x, y, z, c, spec) in _plt.args:
gr.savestate()
mask = gr.uselinespec(spec)
if mask in (0, 1, 3, 4, 5):
gr.polyline([px - 0.07, px - 0.01], [py, py])
if mask & 2:
gr.polymarker([px - 0.06, px - 0.02], [py, py])
gr.restorestate()
gr.settextalign(gr.TEXT_HALIGN_LEFT, gr.TEXT_VALIGN_HALF)
if i < num_labels:
gr.textext(px, py, _plt.kwargs['labels'][i])
i += 1
py -= 0.03
gr.selntran(1)
gr.restorestate()
gr.setviewport(0.1, 0.95, 0.1, 0.95)
gr.setwindow(50, 25000, 0, 100)
gr.setscale(1)
start = time.time()
while time.time() - start < 10:
try:
power = get_spectrum()
peakind = signal.find_peaks_cwt(power, numpy.array([5]))
except (IOError):
continue
gr.clearws()
gr.setlinewidth(1)
gr.setlinecolorind(1)
gr.grid(1, 5, 50, 0, 1, 2)
gr.axes(1, 5, 50, 0, 1, 2, -0.008)
gr.setcharheight(0.020)
gr.text(0.15, 0.965, '100Hz')
gr.text(0.47, 0.965, '1kHz')
gr.text(0.79, 0.965, '10kHz')
gr.setlinecolorind(4)
gr.polyline(f[1:], power[1:])
for p in peakind:
if power[p] > 10:
gr.setlinewidth(2)
gr.setlinecolorind(2)
xe, ye = parabolic(f[p], power, p)
print(xe, ye)
def main():
ymin, ymax = 0.0, 5.0
timestep = 0.03
kp, ki, kd = 0.5, 8.0, 0.001
pid = PIDController(kp,
ki,
kd, (ymax - ymin) / 2,
timestep,
min_output=0,
max_output=1.0)
plant = EmaFilter(alpha=0.7)
init_plot_window(0, 1, 0, 1)
queue_size = 100
t = deque(maxlen=queue_size)
y1 = deque(maxlen=queue_size)
y2 = deque(maxlen=queue_size)
counter = 0
target = 0.0
t0 = time()
while True:
start = time()
if counter % 100 == 0:
target = np.random.randint(low=1, high=5)
pid.setpoint = target / ymax # Normalize to lie inside [0, 1]
# Simulation of measured input
plant_value = plant.ema(pid.output * (ymax - ymin))
pid.update(plant_value / (ymax - ymin), time())
t.append(time() - t0)
y1.append(target)
y2.append(pid.output * (ymax - ymin))
if counter > 0:
xmin, xmax = t[0], t[-1]
# ymin, ymax = min(min(y1), min(y2)), max(max(y1), max(y2))
gr.clearws()
gr.setwindow(xmin, xmax, ymin, ymax)
# Target
gr.setlinewidth(2)
linecolor(0, 0, 1.0)
gr.polyline(t, y1)
# Controller value
gr.setlinewidth(2)
linecolor(1.0, 0, 0)
gr.polyline(t, y2)
gr.setlinewidth(1)
linecolor(0, 0, 0)
draw_axes(1.0, 5.0 / 10, xmin, ymin, x_major=2, y_major=2)
gr.updatews()
counter += 1
sleep(max(timestep - (time() - start), 0.0))
gr.setviewport(0.1, 0.95, 0.1, 0.95)
gr.setwindow(50, 25000, 0, 100)
gr.setscale(1)
start = time.time()
while time.time() - start < 10:
try:
power = get_spectrum()
peakind = signal.find_peaks_cwt(power, np.array([5]))
except (IOError):
continue
gr.clearws()
gr.setlinewidth(1)
gr.setlinecolorind(1)
gr.grid(1, 5, 50, 0, 1, 2)
gr.axes(1, 5, 50, 0, 1, 2, -0.008)
gr.setcharheight(0.020)
gr.text(0.15, 0.965, '100Hz')
gr.text(0.47, 0.965, '1kHz')
gr.text(0.79, 0.965, '10kHz')
gr.setlinecolorind(4)
gr.polyline(f[1:], power[1:])
for p in peakind:
if power[p] > 10:
gr.setlinewidth(2)
gr.setlinecolorind(2)
xe, ye = parabolic(f[p], power, p)
print(xe, ye)
def _draw_axes(kind, pass_=1):
global _plt
viewport = _plt.kwargs['viewport']
vp = _plt.kwargs['vp']
x_tick, x_org, x_major_count = _plt.kwargs['xaxis']
y_tick, y_org, y_major_count = _plt.kwargs['yaxis']
gr.setlinecolorind(1)
gr.setlinewidth(1)
diag = ((viewport[1] - viewport[0])**2 +
(viewport[3] - viewport[2])**2)**0.5
charheight = max(0.018 * diag, 0.012)
gr.setcharheight(charheight)
ticksize = 0.0075 * diag
if kind in ('wireframe', 'surface', 'plot3', 'scatter3', 'trisurf'):
z_tick, z_org, z_major_count = _plt.kwargs['zaxis']
if pass_ == 1:
gr.grid3d(x_tick, 0, z_tick, x_org[0], y_org[1], z_org[0], 2, 0, 2)
gr.grid3d(0, y_tick, 0, x_org[0], y_org[1], z_org[0], 0, 2, 0)
else:
gr.axes3d(x_tick, 0, z_tick, x_org[0], y_org[0], z_org[0],
x_major_count, 0, z_major_count, -ticksize)
gr.axes3d(0, y_tick, 0, x_org[1], y_org[0], z_org[0], 0,
y_major_count, 0, ticksize)
else:
if kind == 'heatmap':
ticksize = -ticksize
else:
gr.grid(x_tick, y_tick, 0, 0, x_major_count, y_major_count)
gr.axes(x_tick, y_tick, x_org[0], y_org[0], x_major_count,
y_major_count, ticksize)
gr.axes(x_tick, y_tick, x_org[1], y_org[1], -x_major_count,
-y_major_count, -ticksize)
if 'title' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_TOP)
gr.textext(0.5 * (viewport[0] + viewport[1]), vp[3],
_plt.kwargs['title'])
gr.restorestate()
if kind in ('wireframe', 'surface', 'plot3', 'scatter3', 'trisurf'):
x_label = _plt.kwargs.get('xlabel', '')
y_label = _plt.kwargs.get('ylabel', '')
z_label = _plt.kwargs.get('zlabel', '')
gr.titles3d(x_label, y_label, z_label)
else:
if 'xlabel' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_BOTTOM)
gr.textext(0.5 * (viewport[0] + viewport[1]),
vp[2] + 0.5 * charheight, _plt.kwargs['xlabel'])
gr.restorestate()
if 'ylabel' in _plt.kwargs:
gr.savestate()
gr.settextalign(gr.TEXT_HALIGN_CENTER, gr.TEXT_VALIGN_TOP)
gr.setcharup(-1, 0)
gr.textext(vp[0] + 0.5 * charheight,
0.5 * (viewport[2] + viewport[3]),
_plt.kwargs['ylabel'])
gr.restorestate()
def drawGR(self):
lwidth = gr.inqlinewidth()
gr.setlinewidth(0.)
PlotAxes.drawGR(self)
gr.setlinewidth(lwidth)
def draw_observation_lines(x, observations):
gr.setlinewidth(1)
for (r, b) in observations:
x1, x2 = x[0], x[0] + r * np.cos(b + x[2])
y1, y2 = x[1], x[1] + r * np.sin(b + x[2])
gr.polyline([x1, x2], [y1, y2])
