Add led_control_task and remove debug_macros. Fix integer overflow problem.
Fix status leds and initial values of pid parameters