Får bättre värden på vinkelfrekvensen. Behöver testas på själva roboten.