#include "I2Cdev.h"
#include "MPU6050.h"
#include <Adafruit_NeoPixel.h>
#include <VL53L0X.h>
#include <LiquidCrystal.h>
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
int mode = 0;
MPU6050 accelgyro; // class default I2C address is 0x68
int16_t ax, ay, az;
int diffAccel = 100;
#define PIN 8
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(1, PIN, NEO_GRB + NEO_KHZ800);
//LiquidCrystal lcd(RS[4], E[6], DB4[11], DB5[12], DB6[13], DB7[14]);
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
char s[16];
VL53L0X sensor;
float height;
// Uncomment this line to use long range mode. This
// increases the sensitivity of the sensor and extends its
// potential range, but increases the likelihood of getting
// an inaccurate reading because of reflections from objects
// other than the intended target. It works best in dark
// conditions.
#define LONG_RANGE
// Uncomment ONE of these two lines to get
// - higher speed at the cost of lower accuracy OR
// - higher accuracy at the cost of lower speed
//#define HIGH_SPEED
#define HIGH_ACCURACY
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
pinMode(9, INPUT_PULLUP);
pixels.begin();
pixels.setBrightness(50);
pixels.setPixelColor(0, pixels.Color(0,0,0));
pixels.show();
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print("x");
lcd.setCursor(9, 0);
lcd.print("y");
lcd.setCursor(0, 1);
lcd.print("Height:");
sensor.init();
sensor.setTimeout(500);
#if defined LONG_RANGE
// lower the return signal rate limit (default is 0.25 MCPS)
sensor.setSignalRateLimit(0.1);
// increase laser pulse periods (defaults are 14 and 10 PCLKs)
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#endif
#if defined HIGH_SPEED
// reduce timing budget to 20 ms (default is about 33 ms)
sensor.setMeasurementTimingBudget(20000);
#elif defined HIGH_ACCURACY
// increase timing budget to 200 ms
sensor.setMeasurementTimingBudget(200000);
#endif
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
if(accelgyro.testConnection()){
pixels.setPixelColor(0, pixels.Color(255,0,0));
pixels.show();
}
// use the code below to change accel/gyro offset values
accelgyro.setXAccelOffset(40); //0
accelgyro.setYAccelOffset(-985); //0
accelgyro.setZAccelOffset(968); //16384
}
void loop() {
//Measurement Mode
while(digitalRead(9) == 0 && mode == 0){
// read raw accel
accelgyro.getAcceleration(&ax, &ay, &az);
lcd.setCursor(1, 0);
lcd.print(dtostrf(ax,6,0,s));
lcd.setCursor(10, 0);
lcd.print(dtostrf(ay,6,0,s));
if(abs(ax) < diffAccel && abs(ay) < diffAccel){
pixels.setPixelColor(0, pixels.Color(0,255,0));
pixels.show();
if (sensor.timeoutOccurred() == 0) {
//Distance measurement
height = sensor.readRangeSingleMillimeters()/10.0 + 0.1;
lcd.setCursor(8, 1);
if(height <= 220.0){
lcd.print(dtostrf(height,5,1,s));
pixels.setPixelColor(0, pixels.Color(0,0,255));
pixels.show();
mode = 1;
}
}
}else{
pixels.setPixelColor(0, pixels.Color(255,0,0));
pixels.show();
}
}
//Mode Reset
if(digitalRead(9) == 1 && mode == 1){
mode = 2;
delay(100);
}
if(digitalRead(9) == 0 && mode == 2){
mode = 0;
delay(100);
}
//Normal Mode
if(digitalRead(9) == 1 && mode == 0){
// read raw accel
accelgyro.getAcceleration(&ax, &ay, &az);
lcd.setCursor(1, 0);
lcd.print(dtostrf(ax,6,0,s));
lcd.setCursor(10, 0);
lcd.print(dtostrf(ay,6,0,s));
if(abs(ax) < diffAccel && abs(ay) < diffAccel){
pixels.setPixelColor(0, pixels.Color(0,255,0));
pixels.show();
}else{
pixels.setPixelColor(0, pixels.Color(255,0,0));
pixels.show();
}
if (sensor.timeoutOccurred() == 0) {
//Distance measurement
height = sensor.readRangeSingleMillimeters()/10.0 + 0.1;
Serial.print(height); Serial.print("\t");
lcd.setCursor(8, 1);
if(height <= 220.0){
lcd.print(dtostrf(height,5,1,s));
}
}
}
}
Comments