149 lines
3.0 KiB
Python
149 lines
3.0 KiB
Python
|
#!/usr/bin/python3
|
||
|
|
|
||
|
|
# OSC komunikacija
|
||
|
|
from pythonosc.osc_server import BlockingOSCUDPServer
|
||
|
|
from pythonosc.dispatcher import Dispatcher
|
||
|
|
from time import time
|
||
|
|
from threading import Thread
|
||
|
|
|
||
|
|
# Risanje grafa
|
||
|
|
import matplotlib
|
||
|
|
matplotlib.use('GTK4Agg')
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
from matplotlib.animation import FuncAnimation
|
||
|
|
|
||
|
|
dispecer = Dispatcher()
|
||
|
|
|
||
|
|
srv = BlockingOSCUDPServer(("127.0.0.1", 57120), dispecer)
|
||
|
|
|
||
|
|
# Zacetna (casovna) tocka
|
||
|
|
zacetek = time()
|
||
|
|
|
||
|
|
# use ggplot style for more sophisticated visuals
|
||
|
|
plt.style.use('ggplot')
|
||
|
|
|
||
|
|
|
||
|
|
size = 1000
|
||
|
|
x_vec = []
|
||
|
|
gx_vec = []
|
||
|
|
gy_vec = []
|
||
|
|
gz_vec = []
|
||
|
|
ax_vec = []
|
||
|
|
ay_vec = []
|
||
|
|
az_vec = []
|
||
|
|
|
||
|
|
x = 0
|
||
|
|
gx = 0
|
||
|
|
gy = 0
|
||
|
|
gz = 0
|
||
|
|
ax = 0
|
||
|
|
ay = 0
|
||
|
|
az = 0
|
||
|
|
|
||
|
|
interval = 60 / 1000
|
||
|
|
razlika = 0
|
||
|
|
|
||
|
|
# Shranim "trenutne" vrednosti
|
||
|
|
def beriSenzor(address, *args):
|
||
|
|
global x, gx, gy, gz, ax, ay, az, razlika
|
||
|
|
|
||
|
|
if (address == '/accel'):
|
||
|
|
ax, ay, az = args
|
||
|
|
|
||
|
|
if (address == '/euler'):
|
||
|
|
gx, gy, gz = args
|
||
|
|
print("euler", args)
|
||
|
|
|
||
|
|
razlika = time() - zacetek
|
||
|
|
if (razlika > interval):
|
||
|
|
izrisi()
|
||
|
|
razlika = razlika % interval
|
||
|
|
|
||
|
|
fig = plt.gcf()
|
||
|
|
#gxs = fig.add_subplot()
|
||
|
|
#gys = fig.add_subplot()
|
||
|
|
|
||
|
|
#gxp, = gxs.plot([])
|
||
|
|
#gyp, = gys.plot([])
|
||
|
|
|
||
|
|
fajl_g = open("graf_g.csv", "w")
|
||
|
|
fajl_g.write("t,gx,gy,gz,rgx,rgy,rgz\n")
|
||
|
|
fajl_a = open("graf_a.csv", "w")
|
||
|
|
fajl_a.write("t,ax,ay,az\n")
|
||
|
|
def izrisi():
|
||
|
|
global x, gx, gy, gz, ax, ay, az, fig, plt, gxp, gyp, zacetek
|
||
|
|
|
||
|
|
# Dodamo casovno tocko
|
||
|
|
x = time() - zacetek
|
||
|
|
#x_vec.append(x)
|
||
|
|
|
||
|
|
fajl_g.write(str(x) + "," + str(gx) + "," + str(gy) + "," + str(gz) + "," + str(gx % 3.14) + "," + str(gy % 3.14) + "," + str(gz % 3.14) + "\n")
|
||
|
|
|
||
|
|
fajl_a.write(str(x) + "," + str(ax) + "," + str(ay) + "," + str(az) + "\n")
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
# Dodamo gyro in accel vrednosti v polja
|
||
|
|
#gx_vec.append(gx)
|
||
|
|
#gy_vec.append(gy)
|
||
|
|
#gz_vec.append(gz)
|
||
|
|
#ax_vec.append(ax)
|
||
|
|
#ay_vec.append(ay)
|
||
|
|
#az_vec.append(az)
|
||
|
|
|
||
|
|
#plt.cla()
|
||
|
|
#yplot.plot(x_vec, gx_vec)
|
||
|
|
#yplot.plot(x_vec, gy_vec)
|
||
|
|
#fig.set_xdata(x_vec)
|
||
|
|
|
||
|
|
#gxp.set_data(x_vec, gx_vec)
|
||
|
|
#gyp.set_data(x_vec, gy_vec)
|
||
|
|
#print("RISEMO", x, gx)
|
||
|
|
#plt.plot(x_vec, gx_vec, label="gyro x")
|
||
|
|
#plt.plot(x_vec, gy_vec)
|
||
|
|
#plt.plot(x_vec, gz_vec)
|
||
|
|
#plt.plot(x_vec, ax_vec)
|
||
|
|
#plt.plot(x_vec, ay_vec)
|
||
|
|
#plt.plot(x_vec, az_vec)
|
||
|
|
|
||
|
|
#plt.pause(0.0001)
|
||
|
|
#fig.canvas.draw()
|
||
|
|
#plt.show()
|
||
|
|
#plt.pause(0.0001)
|
||
|
|
#return gxp,
|
||
|
|
|
||
|
|
#plt.gcf().autofmt_xdate()
|
||
|
|
#plt.tight_layout()
|
||
|
|
#plt.pause(0.001)
|
||
|
|
|
||
|
|
#plt.gcf()
|
||
|
|
#plt.pause(1)
|
||
|
|
#return plt
|
||
|
|
#plt.show()
|
||
|
|
#return plt
|
||
|
|
|
||
|
|
# Animiraj na cca 60hz
|
||
|
|
#animacija = FuncAnimation(fig, izrisi, 500)
|
||
|
|
#animacija = FuncAnimation(plt.gcf(), izrisi, interval=0.05, repeat=True)
|
||
|
|
#print(animacija)
|
||
|
|
#animacija = FuncAnimation(fig, izrisi)
|
||
|
|
|
||
|
|
#plt.show()
|
||
|
|
|
||
|
|
dispecer.map("/*", beriSenzor)
|
||
|
|
# Zacni poslusat senzor
|
||
|
|
print("serving...")
|
||
|
|
srv.serve_forever()
|
||
|
|
#trd = Thread(target=srv.serve_forever).start()
|
||
|
|
|
||
|
|
# Kazi graf
|
||
|
|
#print("Kazi graf!")
|
||
|
|
#plt.tight_layout()
|
||
|
|
#plt.show()
|
||
|
|
#plt.plot([0, 0], [1, 5], [3, 7])
|
||
|
|
#print('kazem', plt)
|
||
|
|
|
||
|
|
# AJD!
|
||
|
|
file.close()
|
||
|
|
print("bye")
|