[CRP] [LF] Implement controller for LF step 1 PID based

[mesmatyi]

Take over controller from jkk_controllers especially the PID part, integrate to framework