Update control algo

Better control based on PID loop to drive position. Works pretty well with good pid values.

Note, temporary disables fiction compensation.