PD control is now working, needs to be tuned