Merge pull request #267 from simplefoc/FEAT_velocity_feadforward

inital implementation of the velocity feed forward
simplefoc · Sep 27, 2023 · 0b53813 · 0b53813
2 parents 7009b48 + 1df9470
commit 0b53813
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
@@ -422,7 +422,7 @@ void BLDCMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit);
+      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
@@ -311,7 +311,7 @@ void StepperMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit);
+      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
@@ -440,4 +440,4 @@ float StepperMotor::angleOpenloop(float target_angle){
   open_loop_timestamp = now_us;
 
   return Uq;
-}
+}