[DEV] Rewrite angular velocity control