diff --git a/.gitconfig b/.gitconfig new file mode 100644 index 00000000..dfa7fc24 --- /dev/null +++ b/.gitconfig @@ -0,0 +1,3 @@ +[user] + email = "aangertdev#gmail.com" + name = Aaron Angert diff --git a/controller_util.py b/controller_util.py index e7177e48..b80e1c57 100755 --- a/controller_util.py +++ b/controller_util.py @@ -6,6 +6,17 @@ import piparty import math import filterpy +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +import numpy as np +from itertools import count +import random + +import multiprocessing +import matplotlib.animation as animation +from multiprocessing import Process +import numpy as np +import time # from filterpy.kalman import ExtendedKalmanFilter #https://thepoorengineer.com/en/ekf-impl/ #https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/11-Extended-Kalman-Filters.ipynb @@ -19,6 +30,8 @@ #another good link: #https://nitinjsanket.github.io/tutorials/attitudeest/kf #http://philsal.co.uk/projects/imu-attitude-estimation + +#complimentory filter #http://www.pieter-jan.com/node/11 #maybe we should try a complimentory filter as from above first! @@ -43,7 +56,7 @@ def Normalize(v): m = VecLen(v) return tuple([ e / m for e in v ]) -async def Loop(plr): +async def Loop(plr,q): print("Acceleration Jerk Gyro") dt = 0.05 # rk = ExtendedKalmanFilter(dim_x=12,dim_z=6) @@ -112,30 +125,159 @@ async def Loop(plr): #we should first get the gyro and accelerometer data and plot it #so we can see how noisy it is + + # plt.pause(0.0001) + # size=100 + # line1 = [] + # x_vec = np.linspace(0,1,size+1)[0:-1] + # y1_data = np.random.randn(len(x_vec)) + # print(x_vec) + # print(y1_data) + # if line1==[]: + + # # this is the call to matplotlib that allows dynamic plotting + # plt.ion() + # fig = plt.figure(figsize=(13,6)) + # # ax = fig.add_subplot(111) + # # create a variable for the line so we can later update it + + # #update plot label/title + # plt.ylabel('Y Label') + # plt.title('Title: {}'.format("acceleration")) + # x = np.arange(0, 2*np.pi, 0.01) + # ax = plt.axes(xlim=(0, 100), ylim=(-1, 1)) + # Color = [ 1 ,0.498039, 0.313725]; + # # line, = ax.plot([], [], '*',color = Color) + # # line1, = ax.plot(x_vec,y1_data,'-o',alpha=0.8) + # x = np.arange(0, 2*np.pi, 0.01) + # line, = ax.plot(x, np.sin(x)) + # # y1_data = [] + # def update_function(i): + # print("UPDATING") + # # line.set_data(y1_data) + # # return line + # # x = np.linspace(0, i+1, i+1) + # # ts = 5*np.cos(x * 0.02 * np.pi) * np.sin(np.cos(x) * 0.02 * np.pi) + # # line.set_data(x, ts) + # line.set_ydata(np.sin(x + i/10.0)) + # return line, + + # def init(): + # print("INITIALIZING") + # line.set_ydata(np.ma.array(x, mask=True)) + # # line.set_data([], []) + # print("ok now") + # return line, + # ani = FuncAnimation(fig, update_function, init_func=init, repeat=False, interval=200, blit=True) + # # plt.pause(0.0001) + # # plt.show() + # plt.pause(0.001) while True: + + for event in plr.get_events(): if event.type != player.EventType.SENSOR: continue - print('\r|%s| = %+.02f |%s| = %+7.02f |%s| = %+2.02f' % ( + + print('acc:|%s| = %+.02f jerk:|%s| = %+7.02f gyro:|%s| = %+2.02f\n' % ( FormatVec(event.acceleration), event.acceleration_magnitude, FormatVec(event.jerk, 7), event.jerk_magnitude, FormatVec(event.gyroscope), VecLen(event.gyroscope)), end='') + q.put(float(event.acceleration_magnitude)) + # y1_data = np.delete(y1_data, [0]) + # y1_data = np.append(y1_data,[float(event.acceleration_magnitude)]) + # # y1_data.pop(0) + # # y1_data.append(event.acceleration_magnitude) + # line1.set_ydata(y1_data) + # # adjust limits if new data goes beyond bounds + # if np.min(y1_data)<=line1.axes.get_ylim()[0] or np.max(y1_data)>=line1.axes.get_ylim()[1]: + # plt.ylim([np.min(y1_data)-np.std(y1_data),np.max(y1_data)+np.std(y1_data)]) + # # this pauses the data so the figure/axis can catch up - the amount of pause can be altered above + # #plt.show() + # plt.pause(0.0001) + #this should be the minimum amount to capture packets await asyncio.sleep(1/30) + +def runGraph(q): + # Parameters + print('show') + x_len = 200 # Number of points to display + y_range = [-10, 10] # Range of possible Y values to display + + # Create figure for plotting + fig = plt.figure() + ax = fig.add_subplot(1, 1, 1) + xs = list(range(0, 200)) + ys = [0] * x_len + ax.set_ylim(y_range) + + # Create a blank line. We will update the line in animate + line, = ax.plot(xs, ys) + + # Add labels + plt.title('TMP102 Temperature over Time') + plt.xlabel('Samples') + plt.ylabel('Temperature (deg C)') + + # This function is called periodically from FuncAnimation + def animate(i, ys): + while not q.empty(): + temp_c = q.get() + # print(q.get()) + # Read temperature (Celsius) from TMP102 + # temp_c = np.random.random(1)*40 + + # Add y to list + ys.append(temp_c) + + # Limit y list to set number of items + ys = ys[-x_len:] + + # Update line with new Y values + line.set_ydata(ys) + + return line, + + + # Set up plot to call animate() function periodically + + ani = animation.FuncAnimation(fig, + animate, + fargs=(ys,), + interval=20, + blit=True) + plt.show() + + + +def MainProgram(): + while 1: + # print('Main program') + time.sleep(0.5) + + def Main(): + q = multiprocessing.Queue() + p = Process(target=runGraph, args=(q,)) + p.start() + # MainProgram() + # piparty.Menu.enable_bt_scanning() + move = psmove.PSMove(0) # move.enable_orientation(True) p1 = player.Player(move) - asyncio.get_event_loop().run_until_complete(Loop(p1)) + asyncio.get_event_loop().run_until_complete(Loop(p1,q)) + p.join() if __name__ == '__main__': Main() diff --git a/test_controller_util.py b/test_controller_util.py new file mode 100755 index 00000000..2e5219d2 --- /dev/null +++ b/test_controller_util.py @@ -0,0 +1,10 @@ +#!/bin/bash + +if [ $UID -ne 0 ]; then + echo "Not root. Using sudo." + exec sudo $0 +fi + +export HOME="/home/pi/JoustMania" +export PYTHONPATH="/home/pi/psmoveapi/build/" +exec /home/pi/JoustMania/venv/bin/python3 /home/pi/JoustMania/controller_util.py