#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
Servo myservo; // create servo object to control a servo
int encoderPin1 = 3;
int encoderPin2 = 2;
volatile int lastEncoded = 0;
volatile long encoderValue = 0;
long lastencoderValue = 0;
int lastMSB = 0;
int lastLSB = 0;
int selectButton = 6;
const int angle = 180;
double angleX;
const double rad = 0.0174533;
double height;
const int trigPin = 10;
const int echoPin = 11;
long duration;
int distance;
void setup() {
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
display.clearDisplay();
myservo.attach(9); // attaches the servo on pin 9 to the servo object
Serial.begin(9600);
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
pinMode(encoderPin1, INPUT);
pinMode(encoderPin2, INPUT);
pinMode(selectButton, INPUT);
digitalWrite(encoderPin1, HIGH);
digitalWrite(encoderPin2, HIGH);
digitalWrite(selectButton, HIGH);
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
}
String S1, S2;
void loop()
{
int count = encoderValue / 2;
if (digitalRead(selectButton) == LOW)
{
encoderValue = 0;
}
digitalWrite(trigPin, LOW); // Clears the trigPin
delayMicroseconds(2);
digitalWrite(trigPin, HIGH); // Sets the trigPin on HIGH state for 10 micro seconds
delayMicroseconds(10);
digitalWrite(trigPin, LOW); // Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance = duration * 0.034 / 2;
if (count < 90)
{
angleX = tan((angle + count) * rad);
height = angleX * (distance + 2);
myservo.write(angle - count); // sets the servo position according to the scaled value
S1 = "D :" + String(distance + 2) + " cm " + "H :" + String(height) + " cm";
S2 = "A :" + String(count) + " deg " + "V :" + String(angleX);
}
display.setCursor(18, 0); //oled display
display.setTextSize(1);
display.setTextColor(WHITE);
display.println("Height & Distance");
display.setCursor(0, 10); //oled display
display.setTextSize(1);
display.setTextColor(WHITE);
display.println(S1);
display.setCursor(0, 22); //oled display
display.setTextSize(1);
display.setTextColor(WHITE);
display.println(S2);
display.display();
delay(500);
display.clearDisplay();
}
void updateEncoder()
{
int MSB = digitalRead(encoderPin1);
int LSB = digitalRead(encoderPin2);
int encoded = (MSB << 1) | LSB;
int sum = (lastEncoded << 2) | encoded;
if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
{
encoderValue ++;
}
if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
{
encoderValue --;
}
lastEncoded = encoded;
}