diff --git a/.vscode/settings.json b/.vscode/settings.json
index cb89910..0c694dc 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,5 +1,6 @@
 {
     "files.associations": {
         "statemachine.h": "c"
-    }
+    },
+    "ros.distro": "humble"
 }
\ No newline at end of file
diff --git a/due_obd2/due_obd2.ino b/due_obd2/due_obd2.ino
index b8e04c1..918d5b0 100644
--- a/due_obd2/due_obd2.ino
+++ b/due_obd2/due_obd2.ino
@@ -9,9 +9,9 @@ Program to create a car monitor display using:
 #include "Arduino.h"
 /* include UTFT library */
 #include <UTFT.h>
-
-#include "obd2_display.h"
+// #include <DueTimer.h>
 #include "obd2_timer.h"
+#include "obd2_display.h"
 #include "statemachine.h"
 
 #define DEBUG 1
@@ -45,7 +45,7 @@ void __state_none(){ /* do nothing */}
 state_t init_state =
     {
         .id = STATE_INIT,
-        .next = STATE_CAR_INFO,
+        .next = STATE_INIT,
         .on_enter = &on_init_enter,
         .on_run = &on_init_run,
         .on_exit = &on_init_exit};
@@ -66,6 +66,14 @@ void on_init_enter()
 {
   display.clrScr();
   Serial.println("Entering init state!");
+  // obd2_TC_init();
+  // Serial.println("init done");
+  // obd2_TC_set_RC_value(1000000); /* 1 million microseconds? */
+  // Serial.println("set RC");
+  // obd2_TC_start();
+  // Serial.print("Timer initialized and started");
+  // Timer3.attachInterrupt(timer_handler);
+  // Timer3.start(50000);
 }
 
 void on_init_run()
@@ -82,7 +90,6 @@ void on_init_exit()
 
 void on_main_enter()
 {
-  obd2_RTT_reset();
   Serial.println("Entering main loop");
 }
 
@@ -91,10 +98,22 @@ void on_main_run()
 
 }
 
-void RTT_Handler()
+// void RTT_Handler()
+// {
+//   flag = 0;
+//   obd2_RTT_reset();
+// }
+
+void TC0_Handler(void)
 {
-  flag = 0;
-  obd2_RTT_reset();
+  #if (DEBUG == 1)
+    Serial.println("Interrupt!");
+  #endif
+}
+
+void timer_handler()
+{
+  Serial.println("Timer yeet");
 }
 
 void setup()
@@ -119,25 +138,30 @@ void setup()
 
   pinMode(LED_BUILTIN, OUTPUT);
 
-  obd2_TC_init();
+  // statemachine_register_state(&init_state, STATE_INIT);
+  // statemachine_register_state(&main_state, STATE_CAR_INFO);
+  // statemachine_init();
+
+
+  obd2_TC_init();
+  Serial.println("init done");
+  obd2_TC_set_RC_value(50000);
+  Serial.println("set RC");
+  obd2_TC_start();
+  Serial.print("Timer initialized and started");
+  // Timer3.attachInterrupt(timer_handler);
+  // Timer3.start(50000);
+  Serial.println("gagfadfgadfsd");
+
 
-  statemachine_register_state(&init_state, STATE_INIT);
-  statemachine_register_state(&main_state, STATE_CAR_INFO);
-  statemachine_init();
 }
 
 void loop()
 {
 
-  statemachine_loop();
-
-  counter_temp++;
-  if (counter_temp == 50)
-  {
-    statemachine_next();
-  }
-
-  delay(100);
+  // statemachine_loop();
+  // Serial.println("Running");
+  // delay(10);
   //   while (Serial1.available())
   //   {
   //     char rec = Serial1.read();
diff --git a/due_obd2/obd2_timer.c b/due_obd2/obd2_timer.c
index 5994e0b..e722227 100644
--- a/due_obd2/obd2_timer.c
+++ b/due_obd2/obd2_timer.c
@@ -1,4 +1,4 @@
-#include "sam3xa/include/component/component_tc.h"
+// #include "sam3xa/include/component/component_tc.h"
 #include "obd2_timer.h"
 
 void obd2_RTT_init(void)
@@ -50,14 +50,48 @@ void obd2_TC_init()
   pmc_set_writeprotect(false);
   // /* set clock source to main clock */
   // pmc_mck_set_source(PMC_MCKR_CSS_MAIN_CLK);
-  /* set the clock to MCK/2, enable the timer/counter 0, enable RC compare trigger and capture mode */
-  TC0->TC_CHANNEL[0].TC_CCR |= TC_CMR_TCCLKS_TIMER_CLOCK1 | TC_CCR_CLKEN | TC_CMR_CPCTRG & ~(TC_CMR_WAVE);
+
+  /* Enable peripheral clock */
+  pmc_enable_periph_clk(TC0_IRQn);
+
+  /* Disable clock  (from arduino15/packages/arduino/hardware/sam/1.6.12/system/libsam/source/tc.c))*/
+  TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_CLKDIS;
+  /* Disable interrupts*/
+  TC0->TC_CHANNEL[0].TC_IDR = 0xFFFFFFFF;
+  /* Clear status register by reading it*/
+  TC0->TC_CHANNEL[0].TC_SR;
+
   /* disable the quadrature decoder, to route the IO pins of TIOA and TIOB directly to the timer counter function (36.6.14.1) */
-  TC0->TC_BMR &= ~(TC_BMR_QDEN);
+//   TC0->TC_BMR &= ~(TC_BMR_QDEN);
+  /* Set wave mode to UP mode with automatic trigger, select wave mode, set the clock to MCK / 2*/
+  TC0->TC_CHANNEL[0].TC_CMR = TC_CMR_WAVSEL_UP_RC | TC_CMR_WAVE | TC_CMR_TCCLKS_TIMER_CLOCK1;
+  /* Enable the RC Compare interrupt*/
+  TC0->TC_CHANNEL[0].TC_IER = TC_IER_CPCS;
+  /* Disable all other interrupts */
+  TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_CPCS;
+}
+
+void obd2_TC_set_RC_value(__UINT32_TYPE__ value)
+{
+  TC0->TC_CHANNEL[0].TC_RC = TC_RC_RC(value);
 }
 
 void obd2_TC_start()
 {
-  /* software trigger the timer reset it and start the clock */
-  TC0->TC_CHANNEL[0].TC_CCR |= TC_CCR_SWTRG;
+
+  /* Clear any still pending IRQs */
+  NVIC_ClearPendingIRQ(TC0_IRQn);
+  /* Enable the IRQ */
+  NVIC_EnableIRQ(TC0_IRQn);
+  /* software trigger the timer resets it and start the clock, also enable the clock */
+  TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;
+}
+
+void obd2_TC_stop()
+{
+    /* stop the clock using an RB load event in capture mode e (LDBSTOP = 1 in TC_CMR (datasheet 36.6.4) */
+    // TC0->TC_CHANNEL[0].TC_CMR |= TC_CMR_CPCSTOP;
+
+    /* Disable the clock*/
+    TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_CLKDIS;
 }
diff --git a/due_obd2/obd2_timer.h b/due_obd2/obd2_timer.h
index 4853818..361a09a 100644
--- a/due_obd2/obd2_timer.h
+++ b/due_obd2/obd2_timer.h
@@ -16,7 +16,9 @@ void obd2_RTT_enable();
 void obd2_RTT_set_period(__UINT32_TYPE__ period);
 
 void obd2_TC_init();
+void obd2_TC_set_RC_value(__UINT32_TYPE__ value);
 void obd2_TC_start();
+void obd2_TC_stop();
 
 #ifdef __cplusplus
 }