diff --git a/right/build/kds/.project b/right/build/kds/.project
index 4844b0b..0da6d24 100644
--- a/right/build/kds/.project
+++ b/right/build/kds/.project
@@ -100,6 +100,16 @@
 			<type>1</type>
 			<locationURI>$%7BPARENT-3-PROJECT_LOC%7D/lib/KSDK_2.0_MK22FN512xxx12/devices/MK22F51212/drivers/fsl_i2c.h</locationURI>
 		</link>
+		<link>
+			<name>drivers/fsl_pit.c</name>
+			<type>1</type>
+			<locationURI>$%7BPARENT-3-PROJECT_LOC%7D/lib/KSDK_2.0_MK22FN512xxx12/devices/MK22F51212/drivers/fsl_pit.c</locationURI>
+		</link>
+		<link>
+			<name>drivers/fsl_pit.h</name>
+			<type>1</type>
+			<locationURI>$%7BPARENT-3-PROJECT_LOC%7D/lib/KSDK_2.0_MK22FN512xxx12/devices/MK22F51212/drivers/fsl_pit.h</locationURI>
+		</link>
 		<link>
 			<name>drivers/fsl_sim.c</name>
 			<type>1</type>
diff --git a/right/src/i2c_watchdog.c b/right/src/i2c_watchdog.c
new file mode 100644
index 0000000..eab60c5
--- /dev/null
+++ b/right/src/i2c_watchdog.c
@@ -0,0 +1,41 @@
+#include "fsl_pit.h"
+#include "fsl_i2c.h"
+#include "fsl_clock.h"
+#include "i2c.h"
+#include "i2c_watchdog.h"
+#include "bridge_protocol_scheduler.h"
+
+#define PIT_I2C_WATCHDOG_HANDLER PIT0_IRQHandler
+#define PIT_I2C_WATCHDOG_IRQ_ID PIT0_IRQn
+#define PIT_SOURCE_CLOCK CLOCK_GetFreq(kCLOCK_BusClk)
+
+static uint32_t prevWatchdogCounter = 0;
+
+/* This function is designed to restart and reinstall the I2C handler
+ * when a disconnection of the left side makes the master I2C bus unresponsive  */
+void PIT_I2C_WATCHDOG_HANDLER(void)
+{
+    if (I2C_Watchdog == prevWatchdogCounter) { // Restart I2C if there hasn't be any interrupt during 1 sec
+        i2c_master_config_t masterConfig;
+        I2C_MasterGetDefaultConfig(&masterConfig);
+        I2C_MasterDeinit(I2C_MAIN_BUS_BASEADDR);
+        uint32_t sourceClock = CLOCK_GetFreq(I2C_MASTER_BUS_CLK_SRC);
+        I2C_MasterInit(I2C_MAIN_BUS_BASEADDR, &masterConfig, sourceClock);
+        InitBridgeProtocolScheduler();
+    }
+
+    prevWatchdogCounter = I2C_Watchdog;
+
+    PIT_ClearStatusFlags(PIT, kPIT_Chnl_0, PIT_TFLG_TIF_MASK);
+}
+
+void InitI2cWatchdog()
+{
+    pit_config_t pitConfig;
+    PIT_GetDefaultConfig(&pitConfig);
+    PIT_Init(PIT, &pitConfig);
+    PIT_SetTimerPeriod(PIT, kPIT_Chnl_0, USEC_TO_COUNT(1000000U, PIT_SOURCE_CLOCK));
+    PIT_EnableInterrupts(PIT, kPIT_Chnl_0, kPIT_TimerInterruptEnable);
+    EnableIRQ(PIT_I2C_WATCHDOG_IRQ_ID);
+    PIT_StartTimer(PIT, kPIT_Chnl_0);
+}
diff --git a/right/src/i2c_watchdog.h b/right/src/i2c_watchdog.h
new file mode 100644
index 0000000..111f6cd
--- /dev/null
+++ b/right/src/i2c_watchdog.h
@@ -0,0 +1,8 @@
+#ifndef __I2C_WATCHDOG_H_
+#define __I2C_WATCHDOG_H_
+
+// Functions:
+
+    extern void InitI2cWatchdog();
+
+#endif
diff --git a/right/src/init_peripherials.c b/right/src/init_peripherials.c
index 2e1a792..2384611 100644
--- a/right/src/init_peripherials.c
+++ b/right/src/init_peripherials.c
@@ -3,6 +3,7 @@
 #include "test_led.h"
 #include "reset_button.h"
 #include "i2c.h"
+#include "i2c_watchdog.h"
 #include "led_driver.h"
 #include "merge_sensor.h"
 #include "led_pwm.h"
@@ -47,25 +48,6 @@ void InitI2c() {
     I2C_MasterInit(I2C_EEPROM_BUS_BASEADDR, &masterConfig, sourceClock);
 }
 
-/* This function is designed to restart and reinstall the I2C handler
- * when a disconnection of the left side makes the master I2C bus unresponsive  */
-volatile uint32_t temp, counter;
-void restartI2C(void) {
-    uint32_t sourceClock;
-    i2c_master_config_t masterConfig;
-
-    temp = I2C_Watchdog; // We take the current value of I2C counter
-    for (counter=0; counter<10000000; counter++);   // This can also be changed for 1 sec delay using PIT
-
-    if (I2C_Watchdog == temp) { // Restart I2C if there hasn't be any interrupt during 1 sec
-        I2C_MasterGetDefaultConfig(&masterConfig);
-        I2C_MasterDeinit(I2C_MAIN_BUS_BASEADDR);
-        sourceClock = CLOCK_GetFreq(I2C_MASTER_BUS_CLK_SRC);
-        I2C_MasterInit(I2C_MAIN_BUS_BASEADDR, &masterConfig, sourceClock);
-        InitBridgeProtocolScheduler();
-    }
-}
-
 void InitPeripherials(void)
 {
     InitResetButton();
@@ -77,4 +59,5 @@ void InitPeripherials(void)
     InitTestLed(); // This function must not be called before LedPwm_Init() or else the UHK won't
                    // reenumerate over USB unless disconnecting it, waiting for at least 4 seconds
                    // and reconnecting it. This is the strangest thing ever!
+    InitI2cWatchdog();
 }
diff --git a/right/src/init_peripherials.h b/right/src/init_peripherials.h
index 4ea38a1..4614856 100644
--- a/right/src/init_peripherials.h
+++ b/right/src/init_peripherials.h
@@ -10,6 +10,5 @@
 // Functions:
 
     void InitPeripherials();
-    void restartI2C();
 
 #endif
diff --git a/right/src/main.c b/right/src/main.c
index b4b813f..20e7015 100644
--- a/right/src/main.c
+++ b/right/src/main.c
@@ -92,7 +92,6 @@ void main() {
 
     while (1) {
         //UpdateUsbReports();
-        restartI2C();
         asm("wfi");
     }
 }