diff --git a/Bench-Data-Firmware/src/main.cpp b/Bench-Data-Firmware/src/main.cpp
index 25d5c7d..7bd2d32 100644
--- a/Bench-Data-Firmware/src/main.cpp
+++ b/Bench-Data-Firmware/src/main.cpp
@@ -2,15 +2,33 @@
 // Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
 // SPDX-License-Identifier: GPL-3.0-or-later
 #include <Arduino.h>
+const byte interruptPin = 2;
+volatile unsigned long timestamp1 = 0;
+volatile unsigned long timestamp2 = 0;
+volatile bool new_rpm = false;
+
+void interrupt(){
+  unsigned long now = micros();
+  timestamp2 = timestamp1;
+  timestamp1 = now;
+  new_rpm = true;
+}
 
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(115200);
+  pinMode(interruptPin, INPUT);
+  attachInterrupt(digitalPinToInterrupt(interruptPin),interrupt,FALLING);
 }
 
 void loop() {
   // put your main code here, to run repeatedly:
-  Serial.println("test");
-  int value = analogRead(A1);
-  Serial.println(value);
-}
\ No newline at end of file
+  if (new_rpm){
+    unsigned long deltat = timestamp1 - timestamp2;
+    float rpmhz = 500000.0 / deltat;
+    new_rpm = false;
+    Serial.println(rpmhz);
+  }
+
+}
+