Split Velocity and Position Controllers

+ add velocity setpoint