broken pid control
reset motors to original convention
Checking functionality WIP