PD control working with signed velocity values. gyro not checked