Working on pid controller
fixed controller.h merge conflict