diff --git a/Software/Core/Inc/stm32h7xx_it.h b/Software/Core/Inc/stm32h7xx_it.h
index ca985e9..f13487c 100644
--- a/Software/Core/Inc/stm32h7xx_it.h
+++ b/Software/Core/Inc/stm32h7xx_it.h
@@ -62,6 +62,7 @@ void FDCAN2_IT0_IRQHandler(void);
 void FDCAN1_IT1_IRQHandler(void);
 void FDCAN2_IT1_IRQHandler(void);
 void TIM8_CC_IRQHandler(void);
+void TIM6_DAC_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/Software/Core/Src/main.c b/Software/Core/Src/main.c
index 4b91a37..b21cb60 100644
--- a/Software/Core/Src/main.c
+++ b/Software/Core/Src/main.c
@@ -50,6 +50,12 @@
 /* Private variables ---------------------------------------------------------*/
 
 /* USER CODE BEGIN PV */
+unsigned int mscounter;
+unsigned int setup_complete;
+
+FDCAN_HandleTypeDef *hMainCAN, *hPeriCAN;
+FDCAN_TxHeaderTypeDef txHeader;
+
 /* Declare buffer in D1 domain SRAM */
 static uint16_t adc_values[NUM_ADC_PINS];
 static uint8_t  dio_values[NUM_DIO_PINS];
@@ -72,6 +78,74 @@ static void MX_NVIC_Init(void);
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
 
+void loop_1kHz() {
+
+  if (!setup_complete)
+    return;
+
+  mscounter++;
+
+  for (int di = 0; di < NUM_DIO_PINS; di++) {
+    dio_values[di] = HAL_GPIO_ReadPin(
+      DIO_PIN_MAP[di].port,
+      DIO_PIN_MAP[di].pin
+    );
+  }
+
+  for (int pi = 0; pi < NUM_TX_PKT; pi++) {
+
+    can_pkt_t* pktinfo = &(CAN_SIGNAL_MAP[pi]);
+
+    if (pktinfo->num_signals < 0)
+      continue;
+
+    if (mscounter % pktinfo->period == 0) {
+
+      txHeader.Identifier = pktinfo->can_id;
+      txHeader.DataLength = pktinfo->dlc;
+      uint64_t txData = 0;
+
+      for (int si = 0; si < pktinfo->num_signals; si++) {
+
+        can_signal_t* signal = &(pktinfo->signals[si]);
+        uint16_t value = 0;
+        switch (signal->type) {
+          case DIN:
+            value = dio_values[signal->channel];
+            break;
+
+          case AIN:
+            value = signal->factor * adc_values[signal->channel];
+            break;
+
+          case FIN:
+            value = wss_flanks[signal->channel];
+            wss_flanks[signal->channel] = 0;
+            break;
+
+          default:
+            break;
+        }
+
+        uint16_t mask = 0xFFFF >> (16 - signal->length);
+        txData |= ((uint64_t) (value & mask)) << (signal->start);
+
+      }
+
+      if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK)
+        Error_Handler();
+
+    }
+
+    if (mscounter >= 500) {
+      mscounter = 0;
+      HAL_GPIO_TogglePin(STATUS_G_GPIO_Port, STATUS_G_Pin);
+    }
+
+  }
+
+}
+
 /* USER CODE END 0 */
 
 /**
@@ -117,8 +191,8 @@ int main(void)
   MX_NVIC_Init();
   /* USER CODE BEGIN 2 */
   
-  FDCAN_HandleTypeDef* hMainCAN = &hfdcan2;
-  //FDCAN_HandleTypeDef* hPeriCAN = &hfdcan1;
+  hMainCAN = &hfdcan2;
+  hPeriCAN = &hfdcan1;
 
   if (HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK)
     Error_Handler();
@@ -126,12 +200,10 @@ int main(void)
   if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_values, NUM_ADC_PINS) != HAL_OK)
     Error_Handler();
 
-  HAL_TIM_Base_Start(&htim6);
+  HAL_TIM_Base_Start_IT(&htim6);
 
   // CAN TX PREP
 
-  FDCAN_TxHeaderTypeDef txHeader;
-
   // Prep the tx frame
   txHeader.ErrorStateIndicator = FDCAN_ESI_PASSIVE;
   txHeader.BitRateSwitch = FDCAN_BRS_OFF;
@@ -170,77 +242,20 @@ int main(void)
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_1);
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_2);
 
+  mscounter = 0;
+  setup_complete = 1;
+
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
-  for (unsigned int mscounter = 0; 1; mscounter++)
+  while(1)
   {
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
-    for (int di = 0; di < NUM_DIO_PINS; di++) {
-      dio_values[di] = HAL_GPIO_ReadPin(
-        DIO_PIN_MAP[di].port,
-        DIO_PIN_MAP[di].pin
-      );
-    }
-
-    for (int pi = 0; pi < NUM_TX_PKT; pi++) {
-
-      can_pkt_t* pktinfo = &(CAN_SIGNAL_MAP[pi]);
-
-      if (pktinfo->num_signals < 0)
-        continue;
-
-      if (mscounter % pktinfo->period == 0) {
-
-        txHeader.Identifier = pktinfo->can_id;
-        txHeader.DataLength = pktinfo->dlc;
-        uint64_t txData = 0;
-
-        for (int si = 0; si < pktinfo->num_signals; si++) {
-
-          can_signal_t* signal = &(pktinfo->signals[si]);
-          uint16_t value = 0;
-          switch (signal->type) {
-            case DIN:
-              value = dio_values[signal->channel];
-              break;
-
-            case AIN:
-              value = signal->factor * adc_values[signal->channel];
-              break;
-
-            case FIN:
-              value = wss_flanks[signal->channel];
-              wss_flanks[signal->channel] = 0;
-              break;
-
-            default:
-              break;
-          }
-
-          uint16_t mask = 0xFFFF >> (16 - signal->length);
-          txData |= ((uint64_t) (value & mask)) << (signal->start);
-
-        }
-
-        if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK)
-          Error_Handler();
-
-      }
-
-      if (mscounter >= 500) {
-        mscounter = 0;
-        HAL_GPIO_TogglePin(STATUS_G_GPIO_Port, STATUS_G_Pin);
-      }
-
-    }
-
-    // TODO: Move all this into a 1kHz timer callback!
-    HAL_Delay(1);
-
+    //HAL_SuspendTick();
+    //HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
   }
   /* USER CODE END 3 */
 }
@@ -318,13 +333,24 @@ static void MX_NVIC_Init(void)
 }
 
 /* USER CODE BEGIN 4 */
-void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
+/*void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
 {
   if (hadc->Instance == ADC1)
   {
     __asm volatile ("NOP");
   }
-}
+}*/
+
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
+
+  //HAL_GPIO_TogglePin(STATUS_B_GPIO_Port, STATUS_B_Pin);
+
+  if (htim != &htim6)
+    return;
+
+  loop_1kHz();
+
+};
 
 /*
  * Input capture timing calculations:
@@ -430,6 +456,11 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *handle, uint32_t RxFifo0ITs)
 
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
+{
+  HAL_ResumeTick();
+}
+
 /* USER CODE END 4 */
 
 /**
diff --git a/Software/Core/Src/stm32h7xx_it.c b/Software/Core/Src/stm32h7xx_it.c
index 50b6234..ee5c1fd 100644
--- a/Software/Core/Src/stm32h7xx_it.c
+++ b/Software/Core/Src/stm32h7xx_it.c
@@ -59,6 +59,7 @@ extern DMA_HandleTypeDef hdma_adc1;
 extern ADC_HandleTypeDef hadc1;
 extern FDCAN_HandleTypeDef hfdcan1;
 extern FDCAN_HandleTypeDef hfdcan2;
+extern TIM_HandleTypeDef htim6;
 extern TIM_HandleTypeDef htim8;
 /* USER CODE BEGIN EV */
 
@@ -300,6 +301,20 @@ void TIM8_CC_IRQHandler(void)
   /* USER CODE END TIM8_CC_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM6 global interrupt, DAC1_CH1 and DAC1_CH2 underrun error interrupts.
+  */
+void TIM6_DAC_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM6_DAC_IRQn 0 */
+
+  /* USER CODE END TIM6_DAC_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim6);
+  /* USER CODE BEGIN TIM6_DAC_IRQn 1 */
+
+  /* USER CODE END TIM6_DAC_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */
diff --git a/Software/Core/Src/tim.c b/Software/Core/Src/tim.c
index d1477d3..7c36f06 100644
--- a/Software/Core/Src/tim.c
+++ b/Software/Core/Src/tim.c
@@ -217,9 +217,9 @@ void MX_TIM6_Init(void)
 
   /* USER CODE END TIM6_Init 1 */
   htim6.Instance = TIM6;
-  htim6.Init.Prescaler = 4800-1;
+  htim6.Init.Prescaler = 480-1;
   htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim6.Init.Period = 2000-1;
+  htim6.Init.Period = 200-1;
   htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
   if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
   {
@@ -334,6 +334,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
   /* USER CODE END TIM6_MspInit 0 */
     /* TIM6 clock enable */
     __HAL_RCC_TIM6_CLK_ENABLE();
+
+    /* TIM6 interrupt Init */
+    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
   /* USER CODE BEGIN TIM6_MspInit 1 */
 
   /* USER CODE END TIM6_MspInit 1 */
@@ -502,6 +506,9 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
   /* USER CODE END TIM6_MspDeInit 0 */
     /* Peripheral clock disable */
     __HAL_RCC_TIM6_CLK_DISABLE();
+
+    /* TIM6 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn);
   /* USER CODE BEGIN TIM6_MspDeInit 1 */
 
   /* USER CODE END TIM6_MspDeInit 1 */
diff --git a/Software/sensor-node.ioc b/Software/sensor-node.ioc
index 6d5b1d6..0d5b347 100644
--- a/Software/sensor-node.ioc
+++ b/Software/sensor-node.ioc
@@ -276,6 +276,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.TIM6_DAC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.TIM8_CC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0.GPIOParameters=GPIO_Label
@@ -633,8 +634,8 @@ TIM4.Period=255
 TIM4.Prescaler=19
 TIM6.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_DISABLE
 TIM6.IPParameters=Prescaler,Period,TIM_MasterOutputTrigger,AutoReloadPreload
-TIM6.Period=2000-1
-TIM6.Prescaler=4800-1
+TIM6.Period=200-1
+TIM6.Prescaler=480-1
 TIM6.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
 TIM8.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
 TIM8.Channel-Input_Capture2_from_TI2=TIM_CHANNEL_2