Test：
Demo：
Code：
#include “Wire.h”
#include “I2Cdev.h”
#include “MPU6050.h”
float axx[2] = {0}, ayy[2] = {0}, azz[2] = {0};
float x = 0, y = 0, z = 0;
int reading = 0;
/* MPU6050 */
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt; //time
int16_t ax, ay, az, gx, gy, gz; //the raw data
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //angle variable
long axo = 0, ayo = 0, azo = 0; //accelerometer offset
long gxo = 0, gyo = 0, gzo = 0; //gyroscopic offset
float pi = 3.1415926;
float AcceRatio = 16384.0; //The ratio of the accelerometer
float GyroRatio = 131.0; //The ratio of gyroscopes
uint8_t n_sample = 8; //The sampling number of the accelerometer filter algorithm
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y
long aax_sum, aay_sum,aaz_sum;
/* RGB LED */
int redpin = 9; //select the pin for the red LED
int greenpin =10; // select the pin for the green LED
int bluepin =11; // select the pin for the blue LED
int val;
int r_val;
int g_val;
int b_val;
/* switch */
const int lzPin = 2;
int lzState;
const int ledPin = 13;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 180000;
void setup() {
pinMode(lzPin, INPUT);
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, LOW);
pinMode(redpin, OUTPUT);
pinMode(greenpin, OUTPUT);
pinMode(bluepin, OUTPUT);
Wire.begin();
Serial.begin(115200);
accelgyro.initialize(); //initialization
unsigned short times = 200;
for(int i=0;i<times;i++){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
axo += ax; ayo += ay; azo += az;
gxo += gx; gyo += gy; gzo += gz;
}
axo /= times; ayo /= times; azo /= times; //Calculate the accelerometer deviation
gxo /= times; gyo /= times; gzo /= times; //Calculate the gyro shift
}
void loop() {
reading = digitalRead(lzPin);
if (reading != lzState) {
lastDebounceTime = millis();
}
unsigned long now = millis(); //time(ms)
dt = (now – lastTime) / 1000.0; //(s)
lastTime = now; //last time(ms)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //raw data
float accx = ax / AcceRatio; //x acceleration
float accy = ay / AcceRatio; //y acceleration
float accz = az / AcceRatio; //z acceleration
aax = atan(accy / accz) * (-180) / pi; //the angle between the Y-axis and z axis
aay = atan(accx / accz) * 180 / pi; //x,z
aaz = atan(accz / accy) * 180 / pi; //z,y
aax_sum = 0;
aay_sum = 0;
aaz_sum = 0;
for(int i=1;i<n_sample;i++){
aaxs[i-1] = aaxs[i];
aax_sum += aaxs[i] * i;
aays[i-1] = aays[i];
aay_sum += aays[i] * i;
aazs[i-1] = aazs[i];
aaz_sum += aazs[i] * i;
}
aaxs[n_sample-1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //chang the angle between 0-90°
aays[n_sample-1] = aay;
aay_sum += aay * n_sample;
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs[n_sample-1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
axx[0] = axx[1];
axx[1] = aax;
ayy[0] = ayy[1];
ayy[1] = aay;
azz[0] = azz[1];
azz[1] = aaz;
x = axx[1] – axx[0];
y = ayy[1] – ayy[0];
z = azz[1] – azz[0];
if ((millis() – lastDebounceTime) < debounceDelay && lastDebounceTime != 0) {
digitalWrite(ledPin, HIGH);
if(x<5 && y<5 && z <5){
reading = digitalRead(lzPin);
if (reading != lzState) {
digitalWrite(ledPin, LOW);
analogWrite(9, 0);
analogWrite(10, 0);
analogWrite(11, 0);
}
}
// red
else if(aaz > -95 && aaz < -85){
r_val = 250;
g_val = 0;
b_val = 0;
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
// green
else if(aaz > 85 && aaz < 95){
r_val = 0;
g_val = 255;
b_val = 0;
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
// blue
else if(aay > -95 && aay < -85){
r_val = 0;
g_val = 0;
b_val = 255;
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
// yellow
else if(aay > 85 && aay < 95){
r_val = 240;
g_val = 205;
b_val = 12;
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
// purple
else if(aax > -95 && aax < -85){
r_val = 237;
g_val = 12;
b_val = 240;
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
// cyan
else if(aax > 85 && aax < 95){
// r_val = 16;
// g_val = 192;
// b_val = 224;
// analogWrite(9, r_val);
// analogWrite(10, g_val);
// analogWrite(11, b_val);
for(val=255; val>0; val–)
{
analogWrite(11, val);
analogWrite(10, 255-val);
analogWrite(9, 128-val);
Serial.println(val);
delay(1);
}
for(val=0; val<255; val++)
{
analogWrite(11, val);
analogWrite(10, 255-val);
analogWrite(9, 128-val);
delay(1);
}
}
// black
else{
analogWrite(9, r_val);
analogWrite(10, g_val);
analogWrite(11, b_val);
}
}
else{
digitalWrite(ledPin, LOW);
analogWrite(9, 0);
analogWrite(10, 0);
analogWrite(11, 0);
}
lzState = LOW;
}
