Added a kd term to the controller and made it able to sweep parameters to find the best PID values