From 482ba92920c4ec3e4be704a6fec98c41ef0edb4d Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Thu, 8 Oct 2026 00:04:35 +0200 Subject: [PATCH] fix(open loop): store the signed shaft velocity in angleOpenloop() angleOpenloop() moves in both directions but always stored a positive shaft_velocity, while velocityOpenloop() stores the signed value. Anything using shaft_velocity (monitoring, and the back-EMF term of estimated_current torque control) therefore sees a positive speed while moving backwards. --- src/common/base_classes/FOCMotor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 214b74ce2..a13cc7561 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -434,8 +434,9 @@ float FOCMotor::angleOpenloop(float target_angle){ // where small position changes are no longer captured by the precision of floats // when the total position is large. if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){ - shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; - shaft_velocity = velocity_limit; + float move_direction = _sign(target_angle - shaft_angle); + shaft_angle += move_direction * abs( velocity_limit ) * Ts; + shaft_velocity = move_direction * abs( velocity_limit ); }else{ shaft_angle = target_angle; shaft_velocity = 0;