First working version without FreeRTOS