PD control, implemented but not stabilizing quad