diff --git a/IO.hpp b/IO.hpp
index 5131870..865fd8d 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -17,9 +17,9 @@
 #define DANCING_ACTION_DELTA_ANGLE 2
 
 #define GAIN_KD 50
-#define ANGULAR_SPEED 10 // °/s
+#define ANGULAR_SPEED 120 // °/s
 
-using std::abs;
+using std::abs, std::signbit;
 
 enum class Axes {
   X,
@@ -125,15 +125,15 @@ class IO {
   {
     static unsigned long prev_time {millis()};
     float err_dir = dir - m_dir;
-    float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
+    float angular_displacement = angular_speed * static_cast<float>(millis() - prev_time) / 1000.0f;
 
-    if(abs(err_dir) < angular_displacement)
+    if(abs(err_dir) < abs(angular_displacement))
     {
       m_dir = dir;
     }
     else
     {
-      m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
+      m_dir += angular_displacement * ((!signbit(err_dir)) * 2 - 1);
     }
 
     prev_time = millis();