Permalink
Switch branches/tags
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
133 lines (107 sloc) 2.86 KB
#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;
}