manual freertos test, cleanup for integration...

-Created a manual FreeRTOS test
-Cleaned main.c for integration
-Fixed some warnings
-Fixed compile errors