/
mmg.ino
132 lines (107 loc) · 2.86 KB
/
mmg.ino
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
#include "Wire.h"
// requires I2Cdev library: https://github.com/jrowberg/i2cdevlib
#include "I2Cdev.h"
// requires NeoPixel library: https://github.com/adafruit/Adafruit_NeoPixel
#include "Adafruit_NeoPixel.h"
// requires MPU-6050 part of the I2Cdev lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
#include "MPU6050.h"
#define NUMLEDS 7
#define LEDPIN 13
int ii;
// sensor
MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float gain;
// filtering
float v[9];
float currval, maxval, smoothval, mag;
uint32_t lastmax;
// LEDs
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMLEDS, LEDPIN, NEO_GRB + NEO_KHZ800);
uint8_t expcor[256]; // exponential brightness correction lookup table
void setup ()
{
Wire.begin();
// build the exponential correction table for LED brightness
build_table(90.0); // higher value -> more linear
// initialize the filter
for (ii=0; ii<9; ii++)
v[ii] = 0.0;
currval = 0.0;
maxval = 0.0;
smoothval = 0.0;
lastmax = millis();
// set up the MPU
sensor.initialize();
gain = 4.0; // higher -> more sensitive
// init led strip
strip.begin();
}
void loop ()
{
// read mpu data
sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// scale ax and feed to filter
mag = step_filter(ax/3276.8);
// find max value in current time window
if (millis()-lastmax < 100)
{
if (fabs(mag) > maxval) maxval = fabs(mag);
} else {
currval = maxval;
maxval = 0.0;
lastmax = millis();
}
// provide smoothing on value
smoothval = 0.9*smoothval + 0.1*currval;
// pick colors for the led strip
for (ii=0; ii<NUMLEDS; ii++)
{
if (smoothval*gain > (ii+1)/(NUMLEDS+1.0))
strip.setPixelColor(ii, expcor[(uint8_t)(ii*60./NUMLEDS)], expcor[(uint8_t)(60.-ii*60./NUMLEDS)], 0);
else
strip.setPixelColor(ii, 1, 1, 1);
}
// send colors out
strip.show();
// the delay is important
// the loop will run at some rate, this rate determines the coefficients
// for the filter. I have measured the time it takes for the loop to
// complete and used this for calculating the coefs.
delay(1);
}
// given a new value x, step the filter forward and return the newest filtered value
// code generated by http://www.schwietering.com/jayduino/filtuino/
// 4th order Butterwoth filter centered on 13 Hz
float step_filter (float x)
{
v[0] = v[1];
v[1] = v[2];
v[2] = v[3];
v[3] = v[4];
v[4] = v[5];
v[5] = v[6];
v[6] = v[7];
v[7] = v[8];
v[8] = (4.392209311882e-5 * x)
+ ( -0.6378838221 * v[0])
+ ( 5.1150034552 * v[1])
+ (-18.2346630494 * v[2])
+ ( 37.7215398561 * v[3])
+ (-49.5091483922 * v[4])
+ ( 42.2118284653 * v[5])
+ (-22.8337680359 * v[6])
+ ( 7.1670057953 * v[7]);
return (v[0] + v[8]) - 4*(v[2] + v[6]) + 6*v[4];
}
void build_table (float gain)
{
float b = 255./(exp(255/gain)-1.0);
for (ii=0; ii<=254; ii++)
{
expcor[ii] = (uint8_t)(b*(exp(ii/gain)-1.0));
}
expcor[0] = 0;
expcor[255] = 255;
}