Added macros for colored output.
Close uart when exit program.
Rename some functions.
Use message filtering for sensor reading and
push messages depending on their frequency.
Adapted structures for filtering.
removed some dependencies.
Hack for compile until sensor stage.
TO DO: CONTROLLER and ACTUATORS ADAPTIONS