diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
index fce552c6..09567aa7 100644
--- a/src/BLDCMotor.cpp
+++ b/src/BLDCMotor.cpp
@@ -418,7 +418,8 @@ void BLDCMotor::move(float new_target) {
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
-      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
       // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 8865f57c..aed56807 100644
--- a/src/StepperMotor.cpp
+++ b/src/StepperMotor.cpp
@@ -307,7 +307,8 @@ void StepperMotor::move(float new_target) {
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
-      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -439,4 +440,4 @@ float StepperMotor::angleOpenloop(float target_angle){
   open_loop_timestamp = now_us;
 
   return Uq;
-}
\ No newline at end of file
+}
diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h
index c499df4a..dc31f3a9 100644
--- a/src/common/base_classes/FOCMotor.h
+++ b/src/common/base_classes/FOCMotor.h
@@ -155,6 +155,7 @@ class FOCMotor
 
     // state variables
     float target; //!< current target value - depends of the controller
+    float feed_forward_velocity = 0.0f; //!< current feed forward velocity
   	float shaft_angle;//!< current motor angle
   	float electrical_angle;//!< current electrical angle
   	float shaft_velocity;//!< current motor velocity