/*
Electronic Level
mpu-6050-level.ino
Uses MPU-6050 IMU
Displays on 128x64 OLED and LED
// Include Wire Library for I2C
#include <Wire.h>
// Include NewLiquidCrystal Library for I2C
#include <LiquidCrystal_I2C.h>
// Define LCD pinout
const int en = 2, rw = 1, rs = 0, d4 = 4, d5 = 5, d6 = 6, d7 = 7, bl = 3, p;
// Define I2C Address - change if reqiuired
const int i2c_addr = 0x3F;
LiquidCrystal_I2C lcd(i2c_addr, en, rw, rs, d4, d5, d6, d7, bl, p);
// Level LEDs
int levelLED_neg1 = 9
int levelLED_neg0 = 10;
int levelLED_level = 11;
int levelLED_pos0 = 12;
int levelLED_pos1 = 13;
//Variables for Gyroscope
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
// Setup timers and temp variables
long loop_timer;
int temp;
// Display counter
int displaycount = 0;
void setup() {
//Start I2C
Wire.begin();
// Set display type as 16 char, 2 rows
lcd.begin(16,2);
// Set Level LEDs as outputs
pinMode(levelLED_neg1, OUTPUT);
pinMode(levelLED_neg0, OUTPUT);
pinMode(levelLED_level, OUTPUT);
pinMode(levelLED_pos0, OUTPUT);
pinMode(levelLED_pos1, OUTPUT);
//Setup the registers of the MPU-6050
setup_mpu_6050_registers();
//Read the raw acc and gyro data from the MPU-6050 1000 times
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
read_mpu_6050_data();
//Add the gyro x offset to the gyro_x_cal variable
gyro_x_cal += gyro_x;
//Add the gyro y offset to the gyro_y_cal variable
gyro_y_cal += gyro_y;
//Add the gyro z offset to the gyro_z_cal variable
gyro_z_cal += gyro_z;
//Delay 3us to have 250Hz for-loop
delay(3);
}
// Divide all results by 1000 to get average offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
// Start Serial Monitor
Serial.begin(115200);
// Init Timer
loop_timer = micros();
}
void loop(){
// Get data from MPU-6050
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
//Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_pitch += gyro_x * 0.0000611;
//Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_roll += gyro_y * 0.0000611;
//If the IMU has yawed transfer the roll angle to the pitch angle
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
//If the IMU has yawed transfer the pitch angle to the roll angle
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
//Calculate the total accelerometer vector
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
//Calculate the pitch angle
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;
//Calculate the roll angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;
//Accelerometer calibration value for pitch
angle_pitch_acc -= 0.0;
//Accelerometer calibration value for roll
angle_roll_acc -= 0.0;
if(set_gyro_angles){
//If the IMU has been running
//Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
//Correct the drift of the gyro roll angle with the accelerometer roll angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else{
//IMU has just started
//Set the gyro pitch angle equal to the accelerometer pitch angle
angle_pitch = angle_pitch_acc;
//Set the gyro roll angle equal to the accelerometer roll angle
angle_roll = angle_roll_acc;
//Set the IMU started flag
set_gyro_angles = true;
}
//To dampen the pitch and roll angles a complementary filter is used
//Take 90% of the output pitch value and add 10% of the raw pitch value
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
//Take 90% of the output roll value and add 10% of the raw roll value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;
//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
// Print to Serial Monitor
//Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
// Increment the display counter
displaycount = displaycount +1;
if (displaycount > 100) {
lcd.clear();
// Print on first row of LCD
lcd.setCursor(0,0);
lcd.print("Pitch: ");
lcd.print(angle_pitch_output);
lcd.setCursor(0,1);
lcd.print("Roll: ");
lcd.print(angle_roll_output);
// Check Angle for Level LEDs
if (angle_pitch_output < -2.01) {
// Turn on Level LED
digitalWrite(levelLED_neg1, HIGH);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output > -2.00) && (angle_pitch_output < -1.01)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, HIGH);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output < 1.00) && (angle_pitch_output > -1.00)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, HIGH);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output > 1.01) && (angle_pitch_output < 2.00)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, HIGH);
digitalWrite(levelLED_pos1, LOW);
} else if (angle_pitch_output > 2.01) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, HIGH);
}
displaycount = 0;
}
while(micros() - loop_timer < 4000);
//Reset the loop timer
loop_timer = micros();
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x6B);
//Set the requested starting register
Wire.write(0x00);
//End the transmission
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1C);
//Set the requested starting register
Wire.write(0x10);
//End the transmission
Wire.endTransmission();
//Configure the gyro (500dps full scale)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1B);
//Set the requested starting register
Wire.write(0x08);
//End the transmission
Wire.endTransmission();
}
void read_mpu_6050_data(){
//Read the raw gyro and accelerometer data
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x3B);
//End the transmission
Wire.endTransmission();
//Request 14 bytes from the MPU-6050
Wire.requestFrom(0x68,14);
//Wait until all the bytes are received
while(Wire.available() < 14);
//Following statements left shift 8 bits, then bitwise OR.
//Turns two 8-bit values into one 16-bit value
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}