diff --git a/source/test/src/test.cpp b/source/test/src/test.cpp
index c219eb0..e70a843 100644
--- a/source/test/src/test.cpp
+++ b/source/test/src/test.cpp
@@ -1,90 +1,44 @@
-/*
-===============================================================================
- Name        : main.c
- Author      : $(author)
- Version     :
- Copyright   : $(copyright)
- Description : main definition
-===============================================================================
-*/
-
 #include "chip.h"
 #include "board.h"
 #include "FreeRTOS.h"
 #include "task.h"
-
 #include <cr_section_macros.h>
+#include "FreeRTOSCPP/Kernel.hpp"
+#include "FreeRTOSCPP/Task.hpp"
 
-/* Sets up system hardware */
-static void prvSetupHardware(void)
-{
-	SystemCoreClockUpdate();
-	Board_Init();
-}
+class LedTask : public FreeRTOS::Task {
+ public:
+  LedTask(const UBaseType_t priority, const char* name)
+      : FreeRTOS::Task(priority, configMINIMAL_STACK_SIZE, name) {}
+  void taskFunction() final;
+ private:
+  int led;
+};
 
-/* LED0 toggle thread */
-static void vLEDTask0(void *pvParameters) {
-	bool LedState = false;
-	while (1) {
-		Board_LED_Set(0, LedState);
+// Task to be created.
+void LedTask::taskFunction() {
+  bool LedState = false;
+  for (;;) {
+		Board_LED_Set(led, LedState);
 		LedState = (bool) !LedState;
-
+		led++;
+		if(led > 2){
+			led = 0;
+		}
 		vTaskDelay(configTICK_RATE_HZ / 2);
-	}
+  }
 }
 
-/* LED1 toggle thread */
-static void vLEDTask1(void *pvParameters) {
-	bool LedState = false;
-	while (1) {
-		Board_LED_Set(1, LedState);
-		LedState = (bool) !LedState;
 
-		vTaskDelay(configTICK_RATE_HZ * 2);
-	}
-}
-
-/* LED2 toggle thread */
-static void vLEDTask2(void *pvParameters) {
-	bool LedState = false;
-	while (1) {
-		Board_LED_Set(2, LedState);
-		LedState = (bool) !LedState;
-
-		vTaskDelay(configTICK_RATE_HZ);
-	}
-}
-
-/*****************************************************************************
- * Public functions
- ****************************************************************************/
-
-/**
- * @brief	main routine for FreeRTOS blinky example
- * @return	Nothing, function should not exit
- */
 int main(void)
 {
-	prvSetupHardware();
+  SystemCoreClockUpdate();
+  Board_Init();
+  // Create a task before starting the kernel.
+  LedTask task1((tskIDLE_PRIORITY + 1UL), "vTaskLed1");
 
-	/* LED1 toggle thread */
-	xTaskCreate(vLEDTask1, "vTaskLed1",
-				configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 1UL),
-				(xTaskHandle *) NULL);
+  // Start the real time kernel with preemption.
+  FreeRTOS::Kernel::startScheduler();
 
-	/* LED2 toggle thread */
-	xTaskCreate(vLEDTask2, "vTaskLed2",
-				configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 1UL),
-				(xTaskHandle *) NULL);
-
-	/* LED0 toggle thread */
-	xTaskCreate(vLEDTask0, "vTaskLed0",
-				configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 1UL),
-				(xTaskHandle *) NULL);
-
-	/* Start the scheduler */
-	vTaskStartScheduler();
-
-	/* Should never arrive here */
-	return 1;
+  return 1;
 }