fixes, working on pid control