diff --git a/keeruti.ino b/keeruti.ino new file mode 100755 index 0000000..aefa52d --- /dev/null +++ b/keeruti.ino @@ -0,0 +1,125 @@ +// Basic demo for accelerometer readings from Adafruit MPU6050 + +#include +#include +#include +#include + +#define LED1_RED 3 +#define LED1_GRN 5 +#define LED1_BLU 6 + +#define LED2_RED 9 +#define LED2_GRN 10 +#define LED2_BLU 11 + +#define LED_BRIGHT 100 + +Adafruit_MPU6050 mpu; + +double x_Buff; +double y_Buff; +double z_Buff; +int roll; +int pitch; +int prevRoll = 0; + +CircularBuffer values; +CircularBuffer averageDifferences; + +void setup(void) { + + pinMode(LED1_RED, OUTPUT); + pinMode(LED1_GRN, OUTPUT); + pinMode(LED1_BLU, OUTPUT); + pinMode(LED2_RED, OUTPUT); + pinMode(LED2_GRN, OUTPUT); + pinMode(LED2_BLU, OUTPUT); + set_led(0,0,0); + + + Serial.begin(115200); + while (!Serial) { + delay(10); // will pause Zero, Leonardo, etc until serial console opens + } + + // Try to initialize! + if (!mpu.begin()) { + //Serial.println("Failed to find MPU6050 chip"); + while (1) { + delay(10); + } + } + + mpu.setAccelerometerRange(MPU6050_RANGE_16_G); + mpu.setGyroRange(MPU6050_RANGE_250_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_10_HZ); + //Serial.println(""); + delay(100); + set_led(0,0,0); +} + +void loop() { + + /* Get new sensor events with the readings */ + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + + /* Print out the values */ + /*Serial.print(a.acceleration.x); + Serial.print(","); + Serial.print(a.acceleration.y); + Serial.print(","); + Serial.print(a.acceleration.z); + Serial.print(", "); + Serial.print(g.gyro.x); + Serial.print(","); + Serial.print(g.gyro.y); + Serial.print(","); + Serial.print(g.gyro.z); + Serial.println(""); + */ + + x_Buff = float(a.acceleration.x); + y_Buff = float(a.acceleration.y); + z_Buff = float(a.acceleration.z); + roll = atan2(y_Buff , z_Buff) * 57.3; + //pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3; + + values.push(roll); + int average = (values[0]+values[1]+values[2]+values[3]+values[4]+values[5]+values[6]+values[7]+values[8]+values[9])/10; + + for (int i=0; i<10; i++) + { + averageDifferences.push(abs(values[i] - average)); + } + + int differences = averageDifferences[0]+averageDifferences[1]+averageDifferences[2]+averageDifferences[3]+averageDifferences[4]+averageDifferences[5]+averageDifferences[6]+averageDifferences[7]+averageDifferences[8]+averageDifferences[9]; + + if (differences < 2 && abs(prevRoll - average)>1) + { + prevRoll = average; + Serial.print(roll+180); + set_led(0,LED_BRIGHT,0); + } + else if (differences >= 2) + { + set_led(LED_BRIGHT,0,0); + } + else + { + set_led(0,0,LED_BRIGHT); + } + + delay(50); +} + +void set_led(uint8_t r, uint8_t g, uint8_t b) +{ + analogWrite(LED1_RED, 255-r); + analogWrite(LED1_GRN, 255-g); + analogWrite(LED1_BLU, 255-b); + analogWrite(LED2_RED, 255-r); + analogWrite(LED2_GRN, 255-g); + analogWrite(LED2_BLU, 255-b); +}