diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -1,6 +1,7 @@
 #include "stm32f0xx_hal.h"
 
 #include "config.h"
+#include "syslib.h"
 #include "states.h"
 #include "ssd1306.h"
 #include "max31855.h"
@@ -16,14 +17,11 @@
 
 
 // Prototypes
-// Move to header file
 void process();
-void SystemClock_Config(void);
 
 therm_settings_t set;
 therm_status_t status;
 
-
 // Globalish setting vars
 SPI_HandleTypeDef hspi1;
 static __IO uint32_t TimingDelay;
@@ -37,17 +35,19 @@ volatile int i=0;
 int main(void)
 {
 
-    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-    HAL_Init();
+    // Initialize HAL
+    hal_init();
 
-    /* Configure the system clock */
-    SystemClock_Config();
+    // Configure the system clock
+    systemclock_config();
 
-    /* Unset bootloader option bytes (if set) */
+    // Unset bootloader option bytes (if set)
     void bootloader_unset(void);
 
-    /* Initialize all configured peripherals */
+    // Init GPIO
     init_gpio();
+
+    // Init USB (TODO: Handle plugged/unplugged with external power)
     MX_USB_DEVICE_Init();
 //    set.usb_plugged = 
 
@@ -55,15 +55,9 @@ int main(void)
     HAL_Delay(1000);
     HAL_GPIO_WritePin(LED_POWER, 1);
 
+    // Enter into bootloader if up button pressed on boot
     if(!HAL_GPIO_ReadPin(SW_UP))
-        bootloader_enter(); // Resets into bootloader
-
-    // TODO: Awesome pwm of power LED 
-
-    // Configure 1ms SysTick (change if more temporal resolution needed) 
-    //RCC_ClocksTypeDef RCC_Clocks;
-    //RCC_GetClocksFreq(&RCC_Clocks);
-    //SysTick_Config(RCC_Clocks.HCLK_Frequency / 1000);
+        bootloader_enter(); 
 
     // Init SPI busses
     init_spi();
@@ -121,33 +115,6 @@ int main(void)
 
 }
 
-// Clock configuration
-void SystemClock_Config(void)
-{
-
-  RCC_OscInitTypeDef RCC_OscInitStruct;
-  RCC_ClkInitTypeDef RCC_ClkInitStruct;
-  RCC_PeriphCLKInitTypeDef PeriphClkInit;
-
-  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48;
-  RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
-  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
-  HAL_RCC_OscConfig(&RCC_OscInitStruct);
-
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
-  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI48;
-  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
-  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
-  HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1);
-
-  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
-  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48;
-  HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
-
-  __SYSCFG_CLK_ENABLE();
-
-}
-
 // PID implementation
 // TODO: Make struct that has the last_temp and i_state in it, pass by ref. Make struct that has other input values maybe.
 int16_t last_pid_temp = 0;