diff --git a/AdaptiveBrightnessFirmware/Inc/adc.h b/AdaptiveBrightnessFirmware/Inc/adc.h
index 1456bb1..99e7d66 100644
--- a/AdaptiveBrightnessFirmware/Inc/adc.h
+++ b/AdaptiveBrightnessFirmware/Inc/adc.h
@@ -24,7 +24,7 @@
 #endif
 
 /* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "init.h"
 
 /* USER CODE BEGIN Includes */
 
diff --git a/AdaptiveBrightnessFirmware/Inc/gpio.h b/AdaptiveBrightnessFirmware/Inc/gpio.h
index 95c08b2..ecaa1f7 100644
--- a/AdaptiveBrightnessFirmware/Inc/gpio.h
+++ b/AdaptiveBrightnessFirmware/Inc/gpio.h
@@ -1,8 +1,8 @@
 /**
   ******************************************************************************
   * File Name          : gpio.h
-  * Description        : This file contains all the functions prototypes for 
-  *                      the gpio  
+  * Description        : This file contains all the functions prototypes for
+  *                      the gpio
   ******************************************************************************
   * @attention
   *
@@ -25,7 +25,7 @@
 #endif
 
 /* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "init.h"
 
 /* USER CODE BEGIN Includes */
 
diff --git a/AdaptiveBrightnessFirmware/Inc/main.h b/AdaptiveBrightnessFirmware/Inc/init.h
similarity index 94%
rename from AdaptiveBrightnessFirmware/Inc/main.h
rename to AdaptiveBrightnessFirmware/Inc/init.h
index 5d2ded7..02cb380 100644
--- a/AdaptiveBrightnessFirmware/Inc/main.h
+++ b/AdaptiveBrightnessFirmware/Inc/init.h
@@ -1,8 +1,8 @@
 /* USER CODE BEGIN Header */
 /**
   ******************************************************************************
-  * @file           : main.h
-  * @brief          : Header for main.c file.
+  * @file           : init.h
+  * @brief          : Header for init.c file.
   *                   This file contains the common defines of the application.
   ******************************************************************************
   * @attention
@@ -20,8 +20,8 @@
 /* USER CODE END Header */
 
 /* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef __MAIN_H
-#define __MAIN_H
+#ifndef __INIT_H
+#define __INIT_H
 
 #ifdef __cplusplus
 extern "C" {
@@ -52,6 +52,7 @@ extern "C" {
 
 /* Exported functions prototypes ---------------------------------------------*/
 void Error_Handler(void);
+void init(void);
 
 /* USER CODE BEGIN EFP */
 
@@ -80,6 +81,6 @@ void Error_Handler(void);
 }
 #endif
 
-#endif /* __MAIN_H */
+#endif /* __INIT_H */
 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/AdaptiveBrightnessFirmware/Inc/usbd_conf.h b/AdaptiveBrightnessFirmware/Inc/usbd_conf.h
index e721ef7..059c7f4 100644
--- a/AdaptiveBrightnessFirmware/Inc/usbd_conf.h
+++ b/AdaptiveBrightnessFirmware/Inc/usbd_conf.h
@@ -31,7 +31,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include "main.h"
+#include "init.h"
 #include "stm32f0xx.h"
 #include "stm32f0xx_hal.h"
 
diff --git a/AdaptiveBrightnessFirmware/Makefile b/AdaptiveBrightnessFirmware/Makefile
index ad3e129..1edeefd 100644
--- a/AdaptiveBrightnessFirmware/Makefile
+++ b/AdaptiveBrightnessFirmware/Makefile
@@ -1,5 +1,6 @@
 ##########################################################################################################################
 # File automatically-generated by tool: [projectgenerator] version: [3.7.1] date: [Sun Jun 28 13:32:11 CEST 2020]
+# and modified by hand
 ##########################################################################################################################
 
 # ------------------------------------------------
@@ -36,7 +37,7 @@ BUILD_DIR = build
 ######################################
 # C sources
 C_SOURCES =  \
-Src/main.c \
+Src/init.c \
 Src/gpio.c \
 Src/adc.c \
 Src/usb_device.c \
@@ -71,6 +72,10 @@ Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c \
 Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c \
 Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c
 
+# C++ sources
+CXX_SOURCES = \
+Src/main.cpp
+
 # ASM sources
 ASM_SOURCES =  \
 startup_stm32f042x6.s
@@ -83,15 +88,17 @@ PREFIX = arm-none-eabi-
 # The gcc compiler bin path can be either defined in make command via GCC_PATH variable (> make GCC_PATH=xxx)
 # either it can be added to the PATH environment variable.
 ifdef GCC_PATH
-CC = $(GCC_PATH)/$(PREFIX)gcc
-AS = $(GCC_PATH)/$(PREFIX)gcc -x assembler-with-cpp
-CP = $(GCC_PATH)/$(PREFIX)objcopy
-SZ = $(GCC_PATH)/$(PREFIX)size
+CC  = $(GCC_PATH)/$(PREFIX)gcc
+CXX = $(GCC_PATH)/$(PREFIX)g++
+AS  = $(GCC_PATH)/$(PREFIX)gcc -x assembler-with-cpp
+CP  = $(GCC_PATH)/$(PREFIX)objcopy
+SZ  = $(GCC_PATH)/$(PREFIX)size
 else
-CC = $(PREFIX)gcc
-AS = $(PREFIX)gcc -x assembler-with-cpp
-CP = $(PREFIX)objcopy
-SZ = $(PREFIX)size
+CC  = $(PREFIX)gcc
+CXX = $(PREFIX)g++
+AS  = $(PREFIX)gcc -x assembler-with-cpp
+CP  = $(PREFIX)objcopy
+SZ  = $(PREFIX)size
 endif
 HEX = $(CP) -O ihex
 BIN = $(CP) -O binary -S
@@ -120,6 +127,11 @@ C_DEFS =  \
 -DUSE_HAL_DRIVER \
 -DSTM32F042x6
 
+# C++ defines
+CXX_DEFS =  \
+-DUSE_HAL_DRIVER \
+-DSTM32F042x6
+
 
 # AS includes
 AS_INCLUDES =
@@ -135,19 +147,34 @@ C_INCLUDES =  \
 -IDrivers/CMSIS/Include \
 -IDrivers/CMSIS/Include
 
+# C++ includes
+CXX_INCLUDES =  \
+-IInc \
+-IDrivers/STM32F0xx_HAL_Driver/Inc \
+-IDrivers/STM32F0xx_HAL_Driver/Inc/Legacy \
+-IMiddlewares/ST/STM32_USB_Device_Library/Core/Inc \
+-IMiddlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc \
+-IDrivers/CMSIS/Device/ST/STM32F0xx/Include \
+-IDrivers/CMSIS/Include \
+-IDrivers/CMSIS/Include
+
 
 # compile gcc flags
 ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
 
 CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
 
+CXXFLAGS = $(MCU) $(CXX_DEFS) $(CXX_INCLUDES) $(OPT) -Wall -Wextra -pedantic -std=c++17 -fdata-sections -ffunction-sections
+
 ifeq ($(DEBUG), 1)
 CFLAGS += -g -gdwarf-2
+CXXFLAGS += -g -gdwarf-2
 endif
 
 
 # Generate dependency information
 CFLAGS += -MMD -MP -MF"$(@:%.o=%.d)"
+CXXFLAGS += -MMD -MP -MF"$(@:%.o=%.d)"
 
 
 #######################################
@@ -159,7 +186,7 @@ LDSCRIPT = STM32F042K6Tx_FLASH.ld
 # libraries
 LIBS = -lc -lm -lnosys
 LIBDIR =
-LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
+LDFLAGS = $(MCU) -specs=nosys.specs -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
 
 # default action: build all
 all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin
@@ -171,6 +198,9 @@ all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET
 # list of objects
 OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o)))
 vpath %.c $(sort $(dir $(C_SOURCES)))
+# list of C++ program objects
+OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(CXX_SOURCES:.cpp=.o)))
+vpath %.cpp $(sort $(dir $(CXX_SOURCES)))
 # list of ASM program objects
 OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(ASM_SOURCES:.s=.o)))
 vpath %.s $(sort $(dir $(ASM_SOURCES)))
@@ -178,11 +208,14 @@ vpath %.s $(sort $(dir $(ASM_SOURCES)))
 $(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR)
 	$(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@
 
+$(BUILD_DIR)/%.o: %.cpp Makefile | $(BUILD_DIR)
+	$(CXX) -c $(CXXFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.cpp=.lst)) $< -o $@
+
 $(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR)
 	$(AS) -c $(CFLAGS) $< -o $@
 
 $(BUILD_DIR)/$(TARGET).elf: $(OBJECTS) Makefile
-	$(CC) $(OBJECTS) $(LDFLAGS) -o $@
+	$(CXX) $(OBJECTS) $(LDFLAGS) -o $@
 	$(SZ) $@
 
 $(BUILD_DIR)/%.hex: $(BUILD_DIR)/%.elf | $(BUILD_DIR)
diff --git a/AdaptiveBrightnessFirmware/Src/main.c b/AdaptiveBrightnessFirmware/Src/init.c
similarity index 92%
rename from AdaptiveBrightnessFirmware/Src/main.c
rename to AdaptiveBrightnessFirmware/Src/init.c
index bb72e33..c71c711 100644
--- a/AdaptiveBrightnessFirmware/Src/main.c
+++ b/AdaptiveBrightnessFirmware/Src/init.c
@@ -1,8 +1,8 @@
 /* USER CODE BEGIN Header */
 /**
   ******************************************************************************
-  * @file           : main.c
-  * @brief          : Main program body
+  * @file           : init.c
+  * @brief          : Init program body
   ******************************************************************************
   * @attention
   *
@@ -19,7 +19,7 @@
 /* USER CODE END Header */
 
 /* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "init.h"
 #include "adc.h"
 #include "usb_device.h"
 #include "gpio.h"
@@ -64,7 +64,7 @@ void SystemClock_Config(void);
   * @brief  The application entry point.
   * @retval int
   */
-int main(void)
+void init(void)
 {
   /* USER CODE BEGIN 1 */
 
@@ -93,16 +93,6 @@ int main(void)
   /* USER CODE BEGIN 2 */
 
   /* USER CODE END 2 */
-
-  /* Infinite loop */
-  /* USER CODE BEGIN WHILE */
-  while (1)
-  {
-    /* USER CODE END WHILE */
-
-    /* USER CODE BEGIN 3 */
-  }
-  /* USER CODE END 3 */
 }
 
 /**
@@ -115,7 +105,7 @@ void SystemClock_Config(void)
   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
   RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
 
-  /** Initializes the CPU, AHB and APB busses clocks 
+  /** Initializes the CPU, AHB and APB busses clocks
   */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI14|RCC_OSCILLATORTYPE_HSI48;
   RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
@@ -126,7 +116,7 @@ void SystemClock_Config(void)
   {
     Error_Handler();
   }
-  /** Initializes the CPU, AHB and APB busses clocks 
+  /** Initializes the CPU, AHB and APB busses clocks
   */
   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                               |RCC_CLOCKTYPE_PCLK1;
@@ -172,7 +162,7 @@ void Error_Handler(void)
   * @retval None
   */
 void assert_failed(uint8_t *file, uint32_t line)
-{ 
+{
   /* USER CODE BEGIN 6 */
   /* User can add his own implementation to report the file name and line number,
      tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
diff --git a/AdaptiveBrightnessFirmware/Src/main.cpp b/AdaptiveBrightnessFirmware/Src/main.cpp
new file mode 100644
index 0000000..4548449
--- /dev/null
+++ b/AdaptiveBrightnessFirmware/Src/main.cpp
@@ -0,0 +1,13 @@
+#include <cstdint>
+
+#include "init.h"
+
+int main()
+{
+    init();
+
+    while(true) {
+    }
+
+    return 0;
+}
diff --git a/AdaptiveBrightnessFirmware/Src/stm32f0xx_hal_msp.c b/AdaptiveBrightnessFirmware/Src/stm32f0xx_hal_msp.c
index dbed325..b2b2c99 100644
--- a/AdaptiveBrightnessFirmware/Src/stm32f0xx_hal_msp.c
+++ b/AdaptiveBrightnessFirmware/Src/stm32f0xx_hal_msp.c
@@ -2,7 +2,7 @@
 /**
   ******************************************************************************
   * File Name          : stm32f0xx_hal_msp.c
-  * Description        : This file provides code for the MSP Initialization 
+  * Description        : This file provides code for the MSP Initialization
   *                      and de-Initialization codes.
   ******************************************************************************
   * @attention
@@ -20,7 +20,7 @@
 /* USER CODE END Header */
 
 /* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "init.h"
 /* USER CODE BEGIN Includes */
 
 /* USER CODE END Includes */
@@ -32,7 +32,7 @@
 
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN Define */
- 
+
 /* USER CODE END Define */
 
 /* Private macro -------------------------------------------------------------*/
diff --git a/AdaptiveBrightnessFirmware/Src/stm32f0xx_it.c b/AdaptiveBrightnessFirmware/Src/stm32f0xx_it.c
index 1a241f0..d888ce1 100644
--- a/AdaptiveBrightnessFirmware/Src/stm32f0xx_it.c
+++ b/AdaptiveBrightnessFirmware/Src/stm32f0xx_it.c
@@ -19,7 +19,7 @@
 /* USER CODE END Header */
 
 /* Includes ------------------------------------------------------------------*/
-#include "main.h"
+#include "init.h"
 #include "stm32f0xx_it.h"
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
@@ -32,7 +32,7 @@
 
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
- 
+
 /* USER CODE END PD */
 
 /* Private macro -------------------------------------------------------------*/
@@ -62,7 +62,7 @@ extern PCD_HandleTypeDef hpcd_USB_FS;
 /* USER CODE END EV */
 
 /******************************************************************************/
-/*           Cortex-M0 Processor Interruption and Exception Handlers          */ 
+/*           Cortex-M0 Processor Interruption and Exception Handlers          */
 /******************************************************************************/
 /**
   * @brief This function handles Non maskable interrupt.