diff --git a/arduino/hello_stepper/HelloController.cpp b/arduino/hello_stepper/HelloController.cpp
index 1d76680..393283a 100644
--- a/arduino/hello_stepper/HelloController.cpp
+++ b/arduino/hello_stepper/HelloController.cpp
@@ -815,9 +815,15 @@ void stepHelloController()
         cmd.stiffness=cmd_in.stiffness;
         cmd.i_contact_pos =cmd_in.i_contact_pos;
         cmd.i_contact_neg =cmd_in.i_contact_neg;
-        g_eff_pos=current_to_effort(abs(cmd.i_contact_pos));
-        g_eff_neg=current_to_effort(-1*abs(cmd.i_contact_neg));
-      
+        g_eff_pos=current_to_effort(cmd.i_contact_pos);
+        g_eff_neg=current_to_effort(cmd.i_contact_neg);
+
+        if (g_eff_pos<g_eff_neg)
+        {
+          g_eff_pos=0;
+          g_eff_neg=0;
+        }
+        
         //If mode has changed manage smooth switchover
         if (cmd.mode!=mode_last)
         {