Added preliminary pd control loop