Commit 69767d0d authored by Tobias Zehntner's avatar Tobias Zehntner

Initial commit

parent cb0388a0
# -*- coding: utf-8 -*-
# © 2018 Tobias Zehntner
# © 2018 Niboo SPRL (https://www.niboo.be/)
# License AGPL-3.0 or later (https://www.gnu.org/licenses/agpl.html).
from math import sqrt, degrees, acos
GYRO_SMOOTHING = 0.01
class ComplementaryFilter:
def get_angles(self, cf_alpha, raw_data, last_cf_data):
"""
Complementary Filter
:param cf_alpha: weighing between Accelerometer(0) and Gyro(1)
:type data: float between 0 and 1
:param raw_data: sensor readings Acc[0] and Gyro [1] {'x', 'y', 'z'}
:type raw_data: list of dicts
:param last_cf_data:
:type last_cf_data:
:return:
:rtype:
"""
if not last_cf_data:
last_data = {'x': 0, 'y': 0, 'z': 0}, {'x': 0, 'y': 0, 'z': 0}
ax_raw = raw_data[0]['x']
ay_raw = raw_data[0]['y']
az_raw = raw_data[0]['z']
gx_raw = raw_data[1]['x']
gy_raw = raw_data[1]['y']
gz_raw = raw_data[1]['z']
acc_vec = sqrt((ax_raw * ax_raw) + (ay_raw * ay_raw) + (az_raw * az_raw))
acc_ang_x = degrees(acos(ax_raw / acc_vec))
acc_ang_y = degrees(acos(ay_raw / acc_vec))
acc_ang_z = degrees(acos(az_raw / acc_vec))
# FIXME last_data needs to have last calculated data, not last sensor data
gx = gx_raw*GYRO_SMOOTHING+last_data[1]['x']
# TODO maybe use list with hardcoded positions for faster processing
cf_data = {}
return cf_data
# -*- coding: utf-8 -*-
# © 2018 Tobias Zehntner
# License AGPL-3.0 or later (https://www.gnu.org/licenses/agpl.html).
import sys
import time
from math import sqrt, degrees, acos
import plotly.plotly as py
from plotly.graph_objs import Scatter, Layout, Figure
from mpu6050 import mpu6050
from swing_plotly import Stream
# Streaming
USERNAME = 'tobiaszehntner'
API_KEY = 'dUqNeznepkMiAH1GSmqK'
STREAM_TOKEN = '5caz2o9tch'
STREAM_X = 50 # points on X axis
# Measuring interval in seconds
DT = 0.02
# Weighing Acc(0) vs Gyro(1) for angles in Complementary Filter
CF_ALPHA = 0.98
# Raw list index
RAW_AX = 0
RAW_AY = 1
RAW_AZ = 2
RAW_GX = 3
RAW_GY = 4
RAW_GZ = 5
def main():
try:
sensor = mpu6050(0x68)
streaming = 'stream' in sys.argv
printing = 'print' in sys.argv
if streaming:
# stream = Stream()
# stream.start(USERNAME, API_KEY, STREAM_TOKEN)
py.sign_in(USERNAME, API_KEY)
stream = py.Stream(STREAM_TOKEN)
stream.open()
trace1 = Scatter(
x=[],
y=[],
stream=dict(
token=STREAM_TOKEN,
maxpoints=200
)
)
layout = Layout(title='Raspberry Pi Streaming Sensor Data')
fig = Figure(data=[trace1], layout=layout)
print py.plot(fig,
filename='Raspberry Pi Streaming Example Values')
# if printing:
# print 'time, ax, ay, az, gx, gy, gz'
gx = False
gy = False
gz = False
cfx = False
cfy = False
cfz = False
# Sensor reading loop
counter = 0
start_time = time.time()
while True:
reading_time = time.time()
reading = sensor.get_all_data()
raw_data = [
reading[0]['x'],
reading[0]['y'],
reading[0]['z'],
reading[1]['x'],
reading[1]['y'],
reading[1]['z'],
]
acc_vec = sqrt((raw_data[RAW_AX] * raw_data[RAW_AX])
+ (raw_data[RAW_AY] * raw_data[RAW_AY])
+ (raw_data[RAW_AZ] * raw_data[RAW_AZ]))
ang_az = degrees(acos(raw_data[RAW_AZ] / acc_vec))
ang_ay = degrees(acos(raw_data[RAW_AY] / acc_vec))
ang_ax = degrees(acos(raw_data[RAW_AX] / acc_vec))
cfz = CF_ALPHA * (cfz + raw_data[RAW_GZ] * DT) + (1 - CF_ALPHA) * ang_az if cfz else ang_az
cfy = CF_ALPHA * (cfy + raw_data[RAW_GY] * DT) + (1 - CF_ALPHA) * ang_ay if cfy else ang_ay
cfx = CF_ALPHA * (cfx + raw_data[RAW_GX] * DT) + (1 - CF_ALPHA) * ang_ax if cfx else ang_ax
gz = raw_data[RAW_GZ] * DT + gz if gz else cfz
gy = raw_data[RAW_GY] * DT + gy if gy else cfy
gx = raw_data[RAW_GX] * DT + gx if gx else cfx
if printing:
print cfz
if streaming:
stream.write({'x': reading_time, 'y': cfz})
# Aim for precise interval
counter += 1
sleep = start_time + (DT * counter) - time.time()
if sleep > 0:
time.sleep(sleep)
except KeyboardInterrupt:
print '\nExiting...'
except Exception, e:
print 'Error: %s' % e
# finally:
# GPIO.cleanup()
if __name__ == '__main__':
# execute only if run as a script
main()
def start(self, username, api_key, stream_token):
py.sign_in(username, api_key)
stream = py.Stream(stream_token)
stream.open()
trace1 = Scatter(
x=[],
y=[],
stream=dict(
token=stream_token,
maxpoints=200
)
)
layout = Layout(title='Raspberry Pi Streaming Sensor Data')
fig = Figure(data=[trace1], layout=layout)
print py.plot(fig, filename='Raspberry Pi Streaming Example Values')
return stream
# -*- coding: utf-8 -*-
# © 2018 Tobias Zehntner
# License AGPL-3.0 or later (https://www.gnu.org/licenses/agpl.html).
import plotly.plotly as py
from plotly.graph_objs import Scatter, Layout, Figure
class Stream:
def start(self, username, api_key, stream_token):
py.sign_in(username, api_key)
stream = py.Stream(stream_token)
stream.open()
trace1 = Scatter(
x=[],
y=[],
stream=dict(
token=stream_token,
maxpoints=200
)
)
layout = Layout(title='Raspberry Pi Streaming Sensor Data')
fig = Figure(data=[trace1], layout=layout)
print py.plot(fig, filename='Raspberry Pi Streaming Example Values')
return stream
import collections as coll
import csv
import matplotlib.pyplot as plt
import numpy as np
import sys
fs = 100.0
dt = 1.0/fs
alpha = 0.02
Sample = coll.namedtuple("Sample",
"accZ accY accX rotZ rotY rotX r acc_angZ acc_angY acc_angX cfZ cfY cfX")
def samples_from_file(fname):
with open(fname, 'r') as f:
next(f) # discard header row
csv_reader = csv.reader(f, dialect='excel')
for i, row in enumerate(csv_reader, 1):
try:
values = [float(x) for x in row]
yield Sample(*values)
except Exception:
lst = list(row)
print("Bad line %d: len %d '%s'" % (i, len(lst), str(lst)))
samples = list(samples_from_file("/Users/tobiaz/WORK/Artwork/2018-01_Aarau/swing-2018/test/my_data4.csv"))
cfx = np.zeros(len(samples))
# Excel formula: =R12
cfx[0] = samples[0].acc_angX
# Excel formula: =0.98*(U12+N13*0.01)+0.02*R13
# Excel: U is cfX N is rotX R is acc_angX
for i, s in enumerate(samples[1:], 1):
cfx[i] = (1.0 - alpha) * (cfx[i-1] + s.rotX*dt) + (alpha * s.acc_angX)
check_line = [s.cfX - cf for s, cf in zip(samples, cfx)]
plt.figure(1)
plt.plot(check_line)
plt.plot(cfx)
plt.show()
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
import RPi.GPIO as GPIO
from mpu6050 import mpu6050
from math import sqrt
import plotly.plotly as py
from plotly.graph_objs import Scatter, Layout, Figure
import time
USERNAME = 'tobiaszehntner'
API_KEY = 'dUqNeznepkMiAH1GSmqK'
STREAM_TOKEN = '5caz2o9tch'
acc = mpu6050(0x68)
# Loop period 20ms
DT = 0.02
SMOOTHING = 4 # amount of readings that are averaged
py.sign_in(USERNAME, API_KEY)
trace1 = Scatter(
x=[],
y=[],
stream=dict(
token=STREAM_TOKEN,
maxpoints=200
)
)
layout = Layout(
title='Raspberry Pi Streaming Sensor Data'
)
fig = Figure(data=[trace1], layout=layout)
print py.plot(fig, filename='Raspberry Pi Streaming Example Values')
i = 0
stream = py.Stream(STREAM_TOKEN)
stream.open()
ax = 0
ay = 0
az = 0
vx = 0
vy = 0
vz = 0
x_list = []
y_list = []
z_list = []
x_angle = 0
y_angle = 0
z_angle = 0
alpha = 0.8
counter = 0
gravity = [0,0,0]
lin_acc = [0,0,0]
try:
# while counter < 9000000:
while True:
# Accelerometer data
acc_data = acc.get_accel_data()
ax = acc_data['x']
ay = acc_data['y']
az = acc_data['z']
# vx += acc_data['x'] * DT
# vy += acc_data['y'] * DT
# vz += acc_data['z'] * DT
# speed = sqrt((vx * vx) + (vy * vy) + (vz * vz))
# gravity[0] = alpha * gravity[0] + (1 - alpha) * ax
# gravity[1] = alpha * gravity[1] + (1 - alpha) * ay
# gravity[2] = alpha * gravity[2] + (1 - alpha) * az
# lax = ax - gravity[0];
# lay = ay - gravity[1];
# laz = az - gravity[2];
# acceleration = sqrt((lin_acc[0] * lin_acc[0]) + (lin_acc[1] * lin_acc[1]) + (lin_acc[2] * lin_acc[2]))
# print '%.3f' % acceleration, ['%.2f' % val for val in lin_acc], ['%.2f' % val for val in gravity]
x_list.append(ax)
y_list.append(ay)
z_list.append(az)
# Gyro
# gyro_data = acc.get_gyro_data()
# x_angle += gyro_data['x'] * DT
# y_angle += gyro_data['y'] * DT
# z_angle += gyro_data['z'] * DT
if counter > SMOOTHING:
smooth_x = sum(x_list)/float(len(x_list))
smooth_y = sum(y_list)/float(len(y_list))
smooth_z = sum(z_list)/float(len(z_list))
gravity[0] = alpha * gravity[0] + (1 - alpha) * smooth_x
gravity[1] = alpha * gravity[1] + (1 - alpha) * smooth_y
gravity[2] = alpha * gravity[2] + (1 - alpha) * smooth_z
lax = smooth_x - gravity[0];
lay = smooth_y - gravity[1];
laz = smooth_z - gravity[2];
acceleration = sqrt((lax * lax) + (lay * lay) + (laz * laz))
del x_list[:]
del y_list[:]
del z_list[:]
counter = 0
stream.write({'x': i, 'y': acceleration})
i += 50
else:
counter += 1
# time.sleep(DT)
except KeyboardInterrupt:
print '\nExiting...'
except Exception, e:
print "Other error or exception occurred: %s" % e
# finally:
# GPIO.cleanup()
import RPi.GPIO as GPIO
from mpu6050 import mpu6050
import plotly.plotly as py
from plotly.graph_objs import Scatter, Layout, Figure
import time
USERNAME = 'tobiaszehntner'
API_KEY = 'dUqNeznepkMiAH1GSmqK'
STREAM_TOKEN = '5caz2o9tch'
acc = mpu6050(0x68)
py.sign_in(USERNAME, API_KEY)
trace1 = Scatter(
x=[],
y=[],
stream=dict(
token=STREAM_TOKEN,
maxpoints=200
)
)
layout = Layout(
title='Raspberry Pi Streaming Sensor Data'
)
fig = Figure(data=[trace1], layout=layout)
print py.plot(fig, filename='Raspberry Pi Streaming Example Values')
i = 0
stream = py.Stream(STREAM_TOKEN)
stream.open()
# counter = 0
try:
# while counter < 9000000:
while True:
temp_data = acc.get_temp()
stream.write({'x': i, 'y': temp_data})
i += 1
time.sleep(0.25)
#counter += 1
#print "Target reached: %d" % counter
except KeyboardInterrupt:
print 'Exiting...'
except:
print "Other error or exception occurred!"
finally:
GPIO.cleanup()
This source diff could not be displayed because it is too large. You can view the blob instead.
import RPi.GPIO as GPIO
from mpu6050 import mpu6050
from math import sqrt
import plotly.plotly as py
from plotly.graph_objs import Scatter, Layout, Figure
import time
USERNAME = 'tobiaszehntner'
API_KEY = 'dUqNeznepkMiAH1GSmqK'
STREAM_TOKEN = '5caz2o9tch'
acc = mpu6050(0x68)
# Loop period 20ms
DT = 0.02
SMOOTHING = 4 # amount of readings that are averaged
i = 0
stream = py.Stream(STREAM_TOKEN)
stream.open()
ax = 0
ay = 0
az = 0
vx = 0
vy = 0
vz = 0
x_list = []
y_list = []
z_list = []
x_angle = 0
y_angle = 0
z_angle = 0
alpha = 0.8
counter = 0
gravity = [0,0,0]
lin_acc = [0,0,0]
try:
# while counter < 9000000:
while True:
# Accelerometer data
acc_data = acc.get_accel_data()
ax = acc_data['x']
ay = acc_data['y']
az = acc_data['z']
# vx += acc_data['x'] * DT
# vy += acc_data['y'] * DT
# vz += acc_data['z'] * DT
# speed = sqrt((vx * vx) + (vy * vy) + (vz * vz))
# gravity[0] = alpha * gravity[0] + (1 - alpha) * ax
# gravity[1] = alpha * gravity[1] + (1 - alpha) * ay
# gravity[2] = alpha * gravity[2] + (1 - alpha) * az
# lax = ax - gravity[0];
# lay = ay - gravity[1];
# laz = az - gravity[2];
# acceleration = sqrt((lin_acc[0] * lin_acc[0]) + (lin_acc[1] * lin_acc[1]) + (lin_acc[2] * lin_acc[2]))
# print '%.3f' % acceleration, ['%.2f' % val for val in lin_acc], ['%.2f' % val for val in gravity]
x_list.append(ax)
y_list.append(ay)
z_list.append(az)
# Gyro
# gyro_data = acc.get_gyro_data()
# x_angle += gyro_data['x'] * DT
# y_angle += gyro_data['y'] * DT
# z_angle += gyro_data['z'] * DT
if counter > SMOOTHING:
smooth_x = sum(x_list)/float(len(x_list))
smooth_y = sum(y_list)/float(len(y_list))
smooth_z = sum(z_list)/float(len(z_list))
gravity[0] = alpha * gravity[0] + (1 - alpha) * smooth_x
gravity[1] = alpha * gravity[1] + (1 - alpha) * smooth_y
gravity[2] = alpha * gravity[2] + (1 - alpha) * smooth_z
lax = smooth_x - gravity[0];
lay = smooth_y - gravity[1];
laz = smooth_z - gravity[2];
acceleration = sqrt((lax * lax) + (lay * lay) + (laz * laz))
del x_list[:]
del y_list[:]
del z_list[:]
counter = 0
else:
counter += 1
# time.sleep(DT)
except KeyboardInterrupt:
print '\nExiting...'
except Exception, e:
print "Other error or exception occurred: %s" % e
# finally:
# GPIO.cleanup()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment