Properly calculate the step
Trial with new equations for rotation