#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <MPU6050_light.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
const int buttonPin = 2; // the number of the pushbutton pin
const int ledPin = 13; // the number of the LED pin
// variables will change:
int buttonState = 0; // variable for reading the pushbutton status
// this is the bitmap, change this variable for your specification.
const unsigned char eyes_front [] PROGMEM = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x1f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x7f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xc0, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xf0, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x01, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00,
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00,
0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00,
0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xff, 0x87, 0xff, 0x80, 0x00, 0xff, 0xe1, 0xff, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xff, 0x03, 0xff, 0x80, 0x00, 0xff, 0xc0, 0xff, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfe, 0x01, 0xff, 0x80, 0x00, 0xff, 0x80, 0x7f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfe, 0x01, 0xff, 0x80, 0x00, 0xff, 0x80, 0x7f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfc, 0x00, 0xff, 0x80, 0x00, 0xff, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfc, 0x00, 0xff, 0x80, 0x00, 0xff, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfc, 0x00, 0xff, 0x80, 0x00, 0xff, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfc, 0x00, 0xff, 0x80, 0x00, 0xff, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfc, 0x00, 0xff, 0x80, 0x00, 0xff, 0x00, 0x3f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x0f, 0xff, 0xfe, 0x01, 0xff, 0x80, 0x00, 0xff, 0x80, 0x7f, 0xff, 0xf8, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xfe, 0x01, 0xff, 0x00, 0x00, 0x7f, 0x80, 0x7f, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0x03, 0xff, 0x00, 0x00, 0x7f, 0xc0, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0x87, 0xff, 0x00, 0x00, 0x7f, 0xe1, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00,
0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00,
0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00,
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00,
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00,
0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x03, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x01, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xf0, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xc0, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x7f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x1f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x01, 0xfc, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
//gyro and acc zmienne
MPU6050 mpu(Wire);
unsigned long timer = 0;
unsigned long timer_X = 0, timer_Y = 0, timer_Z = 0;
int counter_X = 0, counter_Y = 0, counter_Z = 0;
float val_X[5];
float val_Y[5];
float val_Z[5];
int16_t AccX, AccY, AccZ, Temp, GyroX, GyroY, GyroZ;
int16_t gyroX, gyroY, gyroZ;
float temp_X = 0, temp_Y = 0, temp_Z = 0;
float Rot_X = 0, Rot_Y = 0, Rot_Z = 0;
const int MPU6050_addr = 0x68;
void setup() {
Serial.begin(9600);
Wire.begin();
byte status = mpu.begin();
// initialize the LED pin as an output:
pinMode(ledPin, OUTPUT);
// initialize the pushbutton pin as an input:
pinMode(buttonPin, INPUT);
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
Serial.println(F("SSD1306 allocation failed"));
for (;;);
}
delay(500);
mpu.calcOffsets(); // gyro and accelero
// Clear the buffer
// Show the display buffer on the screen. You MUST call display() after
// drawing commands to make them visible on screen!
Wire.begin();
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
display.setTextSize(4);
display.setTextColor(WHITE);
display.setCursor(0, 0);
}
void loop() {
// read the state of the pushbutton value:
buttonState = digitalRead(buttonPin);
// check if the pushbutton is pressed. If it is, the buttonState is HIGH:
if (buttonState == HIGH) {
// turn LED on:
digitalWrite(ledPin, HIGH);
display.clearDisplay();
display.drawBitmap(0, 0, eyes_front, 128, 64, WHITE);
display.display();
} else if (gyroX > 180) {
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(0, 0);
display.println(gyroX);
display.display();
} else {
// turn LED off:
digitalWrite(ledPin, LOW);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(4, 15);
display.println(" . . ");
display.setCursor(6, 23);
display.println(" O O ");
display.display();
delay(500);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(4, 15);
display.println(". . ");
display.setCursor(6, 23);
display.println("O O ");
display.display();
delay(500);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(4, 15);
display.println(" . .");
display.setCursor(6, 23);
display.println(" O O");
display.display();
delay(1000);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(6, 13);
display.println(" - - ");
display.setCursor(4, 21);
display.println(" . . ");
display.setCursor(6, 23);
display.println(" o o ");
display.display();
delay(500);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(6, 23);
display.println(" _ _ ");
display.display();
delay(500);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(6, 23);
display.println(" - - ");
display.display();
delay(1000);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(6, -8);
display.println(" _ _ ");
display.setCursor(4, 15);
display.println(" . . ");
display.setCursor(6, 23);
display.println(" O O ");
display.display();
delay(1000);
display.clearDisplay();
display.setTextColor(WHITE);
display.setCursor(6, 2);
display.println(" \\ / ");
display.setCursor(4, 15);
display.println(" . . ");
display.setCursor(6, 23);
display.println(" O O ");
display.display();
delay(1000);
}
mpu.update();
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 14, true);
AccX = Wire.read() << 8 | Wire.read();
AccY = Wire.read() << 8 | Wire.read();
AccZ = Wire.read() << 8 | Wire.read();
Temp = Wire.read() << 8 | Wire.read();
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
gyroX = (GyroX / 66 ) + 1;
gyroY = (GyroY / 66 ) + 1;
gyroZ = (GyroZ / 66 ) + 1;
Serial.println("Gyro - rps");
Serial.print(gyroX);
Serial.print(", ");
Serial.print(gyroY);
Serial.print(", ");
Serial.print(gyroZ);
Serial.println();
delay(1000);
}