From df5f6de6189b66f557a6b7ee7fe60a7515ff44e8 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 31 Mar 2023 11:29:35 +0200 Subject: [PATCH 1/3] inital implementation of the velocity feed forward --- src/BLDCMotor.cpp | 3 ++- src/StepperMotor.cpp | 3 ++- src/common/base_classes/FOCMotor.h | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index fce552c6..499ac4d3 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_angle_sp = _constrain(shaft_angle_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..48d2cc27 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_angle_sp = _constrain(shaft_angle_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 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 From cbc0be105b556228e0c62d08bf36aa27deb3344e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 24 May 2023 09:57:17 +0200 Subject: [PATCH 2/3] BLDCMotor typo angle instead of velocity --- src/BLDCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 499ac4d3..09567aa7 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -419,7 +419,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 From 1df947091ba4993e90c249cd75e856d20241114c Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Wed, 24 May 2023 09:58:25 +0200 Subject: [PATCH 3/3] StepperMotor typo, angle instead velocity --- src/StepperMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 48d2cc27..aed56807 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -308,7 +308,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; -} \ No newline at end of file +}