Velocity controller close to fully written. Almost ready for debugging.