fixing PD controller to work with angles instead of angular velocities