diff --git a/Projekt_Arduino.ino b/Projekt_Arduino.ino
index cb7d696..7b54db6 100644
--- a/Projekt_Arduino.ino
+++ b/Projekt_Arduino.ino
@@ -3,19 +3,20 @@
 #include "Config.h"
 #include "Display.h"
 #include "Gear.h"
-// #include "Sonar.h"
+#include "Sonar.h"
 #include "RFID.h"
 #include "Engine.h"
 
 Display display;
 GearSelector gear(GEAR_X_PIN, GEAR_Y_PIN, GEAR_THRESH_LOW, GEAR_THRESH_HIGH);
-// Sonar sonar(TRIG_PIN, ECHO_PIN, SONAR_MAX_DIST_CM);
+Sonar sonar(TRIG_AND_ECHO_PIN, SONAR_MAX_DIST_CM);
 RFIDReader rfid(SS_PIN, RST_PIN);
 Engine engine(THROTTLE_PIN, CLUTCH_PIN, BRAKE_PIN);
 
 unsigned long prevUpdate = 0;
-
 bool carEnabled = false;
+unsigned long prevSonarUpdate = 0;
+bool seatBelts = false;
 
 void setup() {
   Serial.begin(9600);
@@ -58,7 +59,13 @@ void loop() {
       display.updateSpeed(engine.getSpeedKmh());
     }
 
-    // long dist = sonar.measure();
-    // Serial.println(dist);
+    if (millis() - prevSonarUpdate >= 100) {
+      prevSonarUpdate = millis();
+      long dist = sonar.measure();
+      if (!seatBelts)
+        if (dist < 200)
+          seatBelts = true;
+        else if (dist >= 200) seatBelts = true;
+    }
   }
 }
diff --git a/Sonar.cpp b/Sonar.cpp
index c0aa458..aaf2cbe 100644
--- a/Sonar.cpp
+++ b/Sonar.cpp
@@ -1,21 +1,26 @@
 #include "Sonar.h"
 
-Sonar::Sonar(int trigPin, int echoPin, long maxDistCm)
-  : _trigPin(trigPin), _echoPin(echoPin),
-    _maxDistCm(maxDistCm), _lastDistance(0) {
-  pinMode(_trigPin, OUTPUT);
-  pinMode(_echoPin, INPUT);
+Sonar::Sonar(int sharedPin, long maxDistCm)
+  : _trigPin(sharedPin),
+    _echoPin(sharedPin),
+    _singlePinMode(true),
+    _maxDistCm(maxDistCm),
+    _lastDistance(0) {
 }
 
 long Sonar::measure() {
-  digitalWrite(_trigPin, LOW);
-  delayMicroseconds(2);
-  digitalWrite(_trigPin, HIGH);
-  delayMicroseconds(10);
-  digitalWrite(_trigPin, LOW);
+  if (_singlePinMode) {
+    _lastDistance = _measureSinglePin();
+  } else {
+    digitalWrite(_trigPin, LOW);
+    delayMicroseconds(2);
+    digitalWrite(_trigPin, HIGH);
+    delayMicroseconds(10);
+    digitalWrite(_trigPin, LOW);
 
-  long duration = pulseIn(_echoPin, HIGH);
-  _lastDistance = duration / 58;
+    long duration = pulseIn(_echoPin, HIGH, 30000UL);
+    _lastDistance = duration / 58;
+  }
 
   if (_lastDistance > _maxDistCm) {
     _lastDistance = _maxDistCm;
@@ -24,6 +29,24 @@ long Sonar::measure() {
   return _lastDistance;
 }
 
+long Sonar::_measureSinglePin() {
+  pinMode(_trigPin, OUTPUT);
+  digitalWrite(_trigPin, LOW);
+  delayMicroseconds(4);
+
+  digitalWrite(_trigPin, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(_trigPin, LOW);
+
+  pinMode(_echoPin, INPUT);
+
+  delayMicroseconds(15);
+
+  long duration = pulseIn(_echoPin, HIGH, 30000UL);
+
+  return duration / 58;
+}
+
 long Sonar::getDistance() const {
   return _lastDistance;
 }
diff --git a/Sonar.h b/Sonar.h
index 751b00f..a8b93d8 100644
--- a/Sonar.h
+++ b/Sonar.h
@@ -3,7 +3,7 @@
 
 class Sonar {
 public:
-  Sonar(int trigPin, int echoPin, long maxDistCm = 400);
+  Sonar(int sharedPin, long maxDistCm = 400);
 
   long measure();
 
@@ -12,6 +12,9 @@ public:
 private:
   int _trigPin;
   int _echoPin;
+  bool _singlePinMode;
   long _maxDistCm;
   long _lastDistance;
+
+  long _measureSinglePin();
 };