From 53835d471463a98986457f89555172935ab61a2f Mon Sep 17 00:00:00 2001
From: Yves Ehrlich <yves.ehrlich@informatik.hs-fulda.de>
Date: Wed, 12 Feb 2020 19:08:50 +0100
Subject: [PATCH] Distanzmessung den Anforderungen entsprechend designed

---
 Code/miniRobot/distance.ino | 64 +++++++++++++++++++++++++++++++++++++
 1 file changed, 64 insertions(+)
 create mode 100644 Code/miniRobot/distance.ino

diff --git a/Code/miniRobot/distance.ino b/Code/miniRobot/distance.ino
new file mode 100644
index 0000000..23b4ec0
--- /dev/null
+++ b/Code/miniRobot/distance.ino
@@ -0,0 +1,64 @@
+#define maxDistance 400
+
+int trig;
+int echo;
+
+//ISR for PCINT2
+ISR(PCINT2_vect) {
+  if((PIND & 64) > 0) {
+    pulseStart = micros();
+  } else {
+    startNewMeasurement = true;
+    pulseLength = micros() - pulseStart;
+    newResult = true;
+  }
+  PCIFR = 0;
+}
+
+void setEchoPins(int pin1, int pin2){
+  
+  trig = pin1;
+  echo = pin2;
+  PCIFR = 0;
+  PCICR |= 0b00000100;
+  PCMSK2 |= (1 << echo);
+
+    // Initializing Trigger Output and Echo Input
+   pinMode(trig, OUTPUT);
+   pinMode(echo, INPUT);
+
+   // Reset the trigger pin and wait a half a second
+   digitalWrite(trig, LOW);
+   delayMicroseconds(500);
+}
+
+
+int16_t calculateDistance(){
+  int16_t result = 1;
+
+  if(pulseLength > 0) {
+    result = (int16_t)(pulseLength / 58);
+  }
+  if(result > maxDistance){
+    result = maxDistance;
+  }
+  newResult = false;
+  //Serial.println(result);
+  return result;
+}
+
+void measureDistance(){
+    
+    digitalWrite(trig, HIGH);
+    // ... wait for 10 µs ...
+    
+    delayMicroseconds(10);
+    // ... put the trigger down ...
+    
+    digitalWrite(trig, LOW); 
+    
+    //Serial.println("messe...");
+
+    startNewMeasurement = false;
+    
+}