Got a basic P-control loop working