Deprez Arduino

/*
"Analóg" távolságmérő "műszer"
"Analog" distance meter "instrument"

*/

#include <Servo.h>

bool measurementFinished = false;
unsigned long startTimestamp;
unsigned long endTimestamp;

// 0,0343 cm / s
// 0,0343 0,0686  0,1372  0,2744  0,5488  1,0976  2,1952  4,3904  8,7808  17,5616 35,1232 70,2464 140,4928  280,9856  561,9712  1123,9424
#define HANGSEBESSEG_SZORZO (1124)
// oda-vissza
#define HANGSEBESSEG_SKALA (16)

#define TAVMERES_IDOZITES_MS  (250)

void triggerDistance(void);
void isr_DistanceMeter(void);

// 0 - 100%
// 180 - 0%
Servo szervo;

const int trigPin = 7;
const int echoPin = 2;
unsigned long distance;
unsigned long triggerSchedule = 0;

void setup() {
  szervo.attach(9);
  pinMode(trigPin, OUTPUT);
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT);
  Serial.begin(115200);
  szervo.write(0);
  delay(1000);
  szervo.write(180);
  attachInterrupt(digitalPinToInterrupt(2), isr_DistanceMeter, CHANGE);
}

void loop() {
  if(triggerSchedule <= millis())
  {
    triggerDistance();
    triggerSchedule = millis() + TAVMERES_IDOZITES_MS;
  }

  if(measurementFinished == true)
  {
    measurementFinished = false;
    distance = calculateDistance(endTimestamp - startTimestamp);
    if(distance > 180)
    {
      szervo.write(0);
    }
    else
    {
      szervo.write(180 - distance);
    }
    Serial.print("Tavolsag: ");
    Serial.println(distance);
  }
}

void triggerDistance(void)
{
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
}

unsigned long calculateDistance(unsigned long duration)
{
  return (duration * HANGSEBESSEG_SZORZO) >> HANGSEBESSEG_SKALA;
}

void isr_DistanceMeter(void)
{
  if(digitalRead(2) == HIGH)
  {
    measurementFinished = false;
    startTimestamp = micros();
  }
  else
  {
    endTimestamp = micros();
    measurementFinished = true;
  }
}