Added testing functions in main for RTOS