diff --git a/blitzercontroller/platformio.ini b/blitzercontroller/platformio.ini
index 4fa4a6d..d7ea76b 100644
--- a/blitzercontroller/platformio.ini
+++ b/blitzercontroller/platformio.ini
@@ -12,3 +12,6 @@
 platform = espressif8266
 board = d1_mini
 framework = arduino
+
+
+monitor_speed = 115200
\ No newline at end of file
diff --git a/blitzercontroller/src/main.cpp b/blitzercontroller/src/main.cpp
index 58b344c..3b179ed 100644
--- a/blitzercontroller/src/main.cpp
+++ b/blitzercontroller/src/main.cpp
@@ -1,9 +1,63 @@
 #include <Arduino.h>
 
+
+
+#define PIN_SW1 D6
+#define PIN_SW2 D7
+volatile boolean sw1_flag = false;
+volatile boolean sw2_flag = false;
+
+unsigned long sw1_lastTime=0;
+unsigned long sw2_lastTime=0;
+
+float calib_distance=0.2; //distance of sensors in meters
+
+#define SWDEBOUNCE 100
+
+void interrupt_sw1();
+void interrupt_sw2();
+float getLastSpeed();
+
 void setup() {
-  // put your setup code here, to run once:
+  pinMode(PIN_SW1, INPUT_PULLUP);
+  pinMode(PIN_SW2, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(PIN_SW1), interrupt_sw1, FALLING);
+  attachInterrupt(digitalPinToInterrupt(PIN_SW2), interrupt_sw2, FALLING);
+
+  Serial.begin(115200);
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+
+  if (sw1_flag){
+    sw1_flag=false;
+    sw1_lastTime=millis();
+    Serial.println("SW1");
+  }
+  if (sw2_flag){
+    sw2_flag=false;
+    sw2_lastTime=millis();
+    Serial.println("SW2");
+
+    float speed=getLastSpeed();
+    Serial.print("Speed="); Serial.print(speed); Serial.println(" m/s");
+    Serial.print("     "); Serial.print(speed*3.6); Serial.println(" km/h");
+  }
+  
+}
+
+void interrupt_sw1() {
+  if (sw1_lastTime+SWDEBOUNCE<millis()){
+    sw1_flag=true;
+  }
+}
+
+void interrupt_sw2() {
+  if (sw2_lastTime+SWDEBOUNCE<millis()){
+    sw2_flag=true;
+  }
+}
+
+float getLastSpeed() {
+  return calib_distance/((sw2_lastTime-sw1_lastTime)/1000.0);
 }
\ No newline at end of file