diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
index e56bd236e788bd60de821f058a2be529f81ef34a..a8cdb069fdc62bbf4c83e604d35354672bbf9f64 100644
--- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
@@ -65,17 +65,25 @@ void AutomaticWingAlgorithm::step()
             modules.get<WindEstimation>()->getWindEstimationScheme());
 
         // Actuate the result
-        if (result >= 0 && result <= 1)
-        {
-            // Activate the servo1 and reset servo2
-            modules.get<Actuators>()->setServo(servo1, result);
-            modules.get<Actuators>()->setServo(servo2, 0);
-        }
-        else if (result < 0 && result <= -1)
+        // To see how to interpret the PI output
+        // https://www.geogebra.org/calculator/xrhwarpz
+        // to system is
+        /*    N ^
+                |
+                |
+                ----> E
+        */
+        if (result > 0)
         {
             // Activate the servo2 and reset servo1
             modules.get<Actuators>()->setServo(servo1, 0);
-            modules.get<Actuators>()->setServo(servo2, result * -1);
+            modules.get<Actuators>()->setServo(servo2, result);
+        }
+        else
+        {
+            // Activate the servo1 and reset servo2
+            modules.get<Actuators>()->setServo(servo1, result * -1);
+            modules.get<Actuators>()->setServo(servo2, 0);
         }
 
         // Log the servo positions
@@ -116,29 +124,6 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
 
     // All angle are computed as angle from the north direction
 
-    // if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0)
-    // {
-    //     // If we are not moving velocityAngle is 0
-    //     velocityAngle = 0;
-    // }
-    // else if (relativeVelocity[0] == 0)
-    // {
-    //     // If we are not moving in the N S axis we interpret velocity[1]
-    //     velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI /
-    //     2;
-    // }
-    // else if (relativeVelocity[1] == 0)
-    // {
-    //     // If we are not moving in the E O axis we interpret velocity[0]
-    //     velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI;
-    // }
-    // else
-    // {
-    //     // angle in radiants of the velocity
-    //     velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
-    // }
-
-    // TODO test if this is safe
     velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
 
     // Compute the angle difference
@@ -149,7 +134,7 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
     result = controller->update(error);
 
     // Convert the result from radians back to degrees
-    result = result * (180.f / Constants::PI);
+    // result = result * (180.f / Constants::PI);
 
     // Flip the servo orientation
     // result *= -1;