//Include Libraries
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <math.h>
//Create LCD Object in the program
//lcd(LCD I2C address, no. of LCD char, no. of segment)
LiquidCrystal_I2C lcd(0x27, 16, 2);
const int MPU_addr = 0x68; //MPU6050 I2C Address
const int minVal = 265; //Min. MPU Raw Value
const int maxVal = 402; //Max. MPU Raw Value
int16_t axis_X, axis_Y, axis_Z; //X,Y,Z axis variables
int x_prev = 0; //store previous x value
int y_prev = 0; //store previous y value
int z_prev = 0; //store previous z value
bool mpuState = LOW; //Initialize MPU state to low.
//Variables for non blocking delay
unsigned long previousMillis1 = 0;
const long interval1 = 50;
const int irSens1 = 2;// interrupt vector of the pin is 0
const int irSens2 = 3; // interrupt vector of the pin is 1
//Use volatile variable as the compiler might see it as unused variable
volatile int irSensState1 = 0;
volatile int irSensState2 = 0;
unsigned int timer1; //current system timer due to irSensor 1
unsigned int timer2; //current system timer due to irSensor 2
//ir sensor flags
bool timerFlag1 = 0;
bool timerFlag2 = 0;
//Variables for speed calculations
float Time = 0;
float speed = 0;
float distance = 0.050; //this is constant and must be the gap distance between two IR sensors
void setup() {
Serial.begin(9600); //Begin serial comms. Uncomment if needed
lcd.init();
lcd.backlight();
lcd.clear();
pinMode(irSens1, INPUT); //Set ir sensor 1 as input
pinMode(irSens2, INPUT); //Set ir sensor 2 as input
//attact an interrupt to the ISR Vector
attachInterrupt(0, irPinInterrupt1, LOW);
attachInterrupt(1, irPinInterrupt2, LOW);
//init_LCD(); //Start LCD
init_Angle_Read(); //Read initial angle
init_Display_MPU(); //Display MPU first start up
lcd_Display_Speed(); //Display Speed
lcd_Display_Angle(); //Display angle
}
void loop() {
if (timerFlag1 == 1 && timerFlag2 == 1) {
if (timer1 > timer2) {
Time = timer1 - timer2;
}
else if (timer1 < timer2) {
Time = timer2 - timer1;
}
Time = Time / 1000.000;
speed = distance / Time;
}
if (speed != speed) {
lcd.setCursor(0, 1);
lcd.print("Speed:");
lcd.print(speed, 3);
lcd.print("m/s");
speed = speed;
}
timerFlag1 = 0;
timerFlag2 = 0;
lcd_Display_Angle();
}
void init_Display_MPU() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
lcd.init();
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print("Arduino");
lcd.setCursor(0, 1);
lcd.print("MPU6050");
delay(2000);
lcd.clear();
}
void init_Angle_Read() {
Wire.beginTransmission(MPU_addr); //begin I2C comms to MPU6050
Wire.write(0x3B); //Read X,Y,Z raw command
Wire.endTransmission(false); //terminate I2C comms
Wire.requestFrom(MPU_addr, 14, true); //Request 14bit acceleration values
//Store X Y Z raw values
axis_X = Wire.read() << 8 | Wire.read();
axis_Y = Wire.read() << 8 | Wire.read();
axis_Z = Wire.read() << 8 | Wire.read();
//Convert raw values to angle in radians
int xAng = map(axis_X, minVal, maxVal, -90, 90);
int yAng = map(axis_Y, minVal, maxVal, -90, 90);
int zAng = map(axis_Z, minVal, maxVal, -90, 90);
//Convert radians to degrees
int x_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
int y_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
int z_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
//Store previous X,Y,Z values
x_prev = x_curr;
y_prev = y_curr;
z_prev = z_curr;
}
void lcd_Display_Angle() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
axis_X = Wire.read() << 8 | Wire.read();
axis_Y = Wire.read() << 8 | Wire.read();
axis_Z = Wire.read() << 8 | Wire.read();
int xAng = map(axis_X, minVal, maxVal, -90, 90);
int yAng = map(axis_Y, minVal, maxVal, -90, 90);
int zAng = map(axis_Z, minVal, maxVal, -90, 90);
//int x_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using X axis
//int y_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using Y axis
int z_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using Z axis
/*
//For X-axis reference. //Comment this block of code if not using X axis
if(x_curr >=0 && x_curr <=90){
if(x_curr != x_prev){
lcd.setCursor(0, 0);
lcd.print("Angle:");
lcd.print(x_curr);
x_prev = x_curr;
}
}
*/
/*
//For Y-axis reference. //Comment this block of code if not using Y axis
if(y_curr >=0 && y_curr <=90){
if(y_curr != y_prev){
lcd.setCursor(0, 0);
lcd.print("Angle:");
lcd.print(y_curr);
y_prev = y_curr;
}
}
*/
//For Z-axis reference. //Comment this block of code if not using Z axis
if (z_curr >= 0 && z_curr <= 90) {
if (z_curr != z_prev) {
lcd.setCursor(0, 0);
lcd.print("Angle:");
lcd.print(z_curr);
z_prev = z_curr;
}
}
}
void lcd_Display_Speed() {
if (timerFlag1 == 1 && timerFlag2 == 1) {
if (timer1 > timer2) {
Time = timer1 - timer2;
}
else if (timer1 < timer2) {
Time = timer2 - timer1;
}
//Calculate speed
Time = Time / 1000.000;
speed = distance / Time;
}
if (speed != speed) {
lcd.setCursor(0, 1);
lcd.print("Speed:");
lcd.print(speed, 3);
lcd.print("m/s");
speed = speed;
}
//Reset timer flag
timerFlag1 = 0;
timerFlag2 = 0;
}
void irPinInterrupt1() {
timer1 = millis();
timerFlag1 = 1;
}
void irPinInterrupt2() {
timer2 = millis();
timerFlag2 = 1;
}