diff --git a/platformio.ini b/platformio.ini
index 7b0da94..c9aa938 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -28,9 +28,8 @@ framework = arduino
 src_filter = +<receiver.cpp>
 
 monitor_baud = 115200
-upload_speed = 921600
+; upload_speed = 921600
 
 lib_deps =
-  # Using a library name
   ServoESP32
   DallasTemperature
diff --git a/src/mac_config.h b/src/mac_config.h
index b63340e..f0ab08a 100644
--- a/src/mac_config.h
+++ b/src/mac_config.h
@@ -1,7 +1,7 @@
 //mac addresses
 
 // reciever mac:
-uint8_t mac_receiver[] = {0x30, 0xae, 0xa4, 0x05, 0x82, 0x15};
+uint8_t mac_receiver[] = {0xB4, 0xE6, 0x2D, 0x8C, 0x34, 0xAE};
 
 // remote macStr
 uint8_t mac_remote[] = {0x24, 0x0a, 0xc4, 0x82, 0x51, 0x70};
diff --git a/src/receiver.cpp b/src/receiver.cpp
index 1a5824f..8f0e890 100644
--- a/src/receiver.cpp
+++ b/src/receiver.cpp
@@ -36,14 +36,16 @@ esp_now_peer_info_t remote;
 
 float temperature = 0;
 float voltage = 0;
-float speed = 0;
+float kmh = 0;
 
 uint8_t sendTemperature = 0;
 uint8_t sendTemperatureDecimals = 0;
 uint8_t sendVoltage = 0;
 uint8_t sendVoltageDecimals = 0;
-uint8_t sendRPMupper = 1;
-uint8_t sendRPMlower = 2;
+uint8_t sendSpeed = 0;
+uint8_t sendSpeedDecimals = 0;
+
+#include "rpm.h"
 
 //#define pairingMode
 #define CONNECTION_TIMEOUT 300
@@ -80,9 +82,9 @@ void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
 
 // Answer with response
 
-  const uint8_t respData[] = { sendVoltage, sendVoltageDecimals, sendTemperature, sendTemperatureDecimals, sendRPMupper, sendRPMlower };
+  const uint8_t respData[] = { sendVoltage, sendVoltageDecimals, sendTemperature, sendTemperatureDecimals, sendSpeed, sendSpeedDecimals };
   Serial.print("Sending RESPONSE.... ");
-  esp_err_t result = esp_now_send(mac_addr, respData, sizeof(data));
+  esp_err_t result = esp_now_send(mac_addr, respData, sizeof(respData));
   if (result == ESP_OK) {
     Serial.println("Success");
   } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
@@ -192,8 +194,8 @@ bool manageRemote() {
 void checkTemperature() {
   sensors.requestTemperatures();   // Send the command to get temperatures
   temperature = sensors.getTempCByIndex(0);
-  Serial.print("Temp: ");
-  Serial.println(temperature);
+  //Serial.print("Temp: ");
+  //Serial.println(temperature);
   if(temperature < 35) digitalWrite(fanRelais, LOW);
   if(temperature > 40) digitalWrite(fanRelais, HIGH);
   sendTemperature = abs(floor(temperature));
@@ -202,9 +204,9 @@ void checkTemperature() {
 
 void checkVoltage() {
   digitalWrite(voltageEnable, HIGH);
-  Serial.print("Voltage: ");
+  //Serial.print("Voltage: ");
   int value = analogRead(voltagePin);
-  Serial.println(value);
+  //Serial.println(value);
 
 
   float batteryVoltage = 0.0;
@@ -220,8 +222,8 @@ void checkVoltage() {
 
   sendVoltage = abs(floor(batteryVoltage));
   sendVoltageDecimals = (batteryVoltage - sendVoltage) * 100;
-  Serial.print("Voltage: ");
-  Serial.println(batteryVoltage);
+  //Serial.print("Voltage: ");
+  //Serial.println(batteryVoltage);
   digitalWrite(voltageEnable, LOW);  // change to low
 }
 
@@ -237,6 +239,8 @@ void setup() {
   pinMode(voltageEnable, OUTPUT);
   pinMode(fanRelais, OUTPUT);
 
+  initRPMPins();
+
   // Init escs, min and max value similar as Traxxas TQI 1100, 1900
   // chanel, minAngel, maxAngel, minPulseWidth, maxPulseWidth
   esc1.attach(esc1pin, 0, 0, 255, 1100, 1900);
@@ -244,6 +248,15 @@ void setup() {
 
   sensors.begin();
 
+  xTaskCreatePinnedToCore(
+    measureRpm,
+    "rpm task",
+    1000,
+    NULL,
+    1,
+    &rpmTaskHandle,
+    0);
+
 
   //Set device in AP mode to begin with
   WiFi.mode(WIFI_AP);
diff --git a/src/remote.cpp b/src/remote.cpp
index a9dfb27..5cac7a8 100644
--- a/src/remote.cpp
+++ b/src/remote.cpp
@@ -565,7 +565,6 @@ void drawPage() {
 
   switch (displayData) {
   case 0:
-    //value = ratioRpmSpeed * data.rpm;
     first = boardData[B_TEMP];
     last = boardData[B_TEMP_D];
     suffix = "C";
@@ -573,8 +572,6 @@ void drawPage() {
     decimals = 2;
     break;
   case 1:
-    //value = ratioPulseDistance * data.tachometerAbs;
-    // TODO : check how that will work once hal is wired
     first = boardData[B_SPEED];
     last = boardData[B_SPEED_D];
     suffix = "KmH";
diff --git a/src/rpm.h b/src/rpm.h
new file mode 100644
index 0000000..f3e8b70
--- /dev/null
+++ b/src/rpm.h
@@ -0,0 +1,90 @@
+#define halBlueLeft 2
+#define halYellowLeft 22
+#define halGreenLeft 19
+
+#define halBlueRight 23
+#define halYellowRight 18
+#define halGreenRight 5
+
+#define wheelCircumference 26.062
+
+TaskHandle_t rpmTaskHandle;
+
+int rpmRight = 0;
+int rpmLeft = 0;
+
+int lastState[2] = {1, 1};
+long lastTime[2] = {0, 0};
+int counter[2] = {0, 0};
+
+long rpmTimer = 0;
+
+// Zero states are non existant
+int motorPosition[2][2][2] {
+  {
+    {0, 1},
+    {3, 2}
+  },
+  {
+    {5, 6},
+    {4, 0}
+  }
+};
+// old state, new state
+int relativeLookup[6][6] {
+  { 0, 1, 2, 3,-2,-1}, // 1
+  {-1, 0, 1, 2, 3,-1}, // 2
+  {-2,-1, 0, 1, 2, 3}, // 3
+  {-3,-2,-1, 0, 1, 2}, // 4
+  { 1,-3,-2,-1, 0, 1}, // 5
+  { 1, 2,-3,-2,-1, 0}  // 6
+};
+
+int getState(bool isLeft) {
+  if(isLeft) {
+    return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
+  } else {
+    return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
+  }
+}
+
+void initRPMPins() {
+  pinMode(halBlueLeft, INPUT_PULLUP);
+  pinMode(halYellowLeft, INPUT_PULLUP);
+  pinMode(halGreenLeft, INPUT_PULLUP);
+
+  pinMode(halBlueRight, INPUT_PULLUP);
+  pinMode(halYellowRight, INPUT_PULLUP);
+  pinMode(halGreenRight, INPUT_PULLUP);
+}
+
+void measureRpm(void * parameter) {
+  while(true) {
+    if(millis()-rpmTimer>250) {
+      rpmRight = counter[false]*4;
+      rpmLeft = counter[true]*4;
+      kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 60/1000;
+
+      sendSpeed = abs(floor(kmh));
+      sendSpeedDecimals = (kmh - sendSpeed) * 100;
+      Serial.print("remote "); Serial.println(kmh);
+      counter[true] = 0;
+      counter[false] = 0;
+      rpmTimer = millis();
+    }
+
+    int state = getState(false);
+    if(lastState[false] != state) {
+      counter[false] = counter[false] + relativeLookup[lastState[false]-1][state-1];
+      lastState[false] = state;
+    }
+
+    state = getState(true);
+    if(lastState[true] != state) {
+      counter[true] = counter[true] + relativeLookup[lastState[true]-1][state-1];
+      lastState[true] = state;
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(1));
+  }
+}
\ No newline at end of file