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