This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit ce53f0fc4cd85e2dda52da523dc51491efd1a925
Author: raiden00pl <raide...@railab.me>
AuthorDate: Thu Nov 9 12:22:31 2023 +0100
examples/foc: add torque saturation
---
examples/foc/Kconfig | 17 +++++++++++------
examples/foc/foc_motor_b16.c | 8 ++++++++
examples/foc/foc_motor_b16.h | 1 +
examples/foc/foc_motor_f32.c | 8 ++++++++
examples/foc/foc_motor_f32.h | 1 +
5 files changed, 29 insertions(+), 6 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index cabd09d37..abeee8be2 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -187,6 +187,17 @@ config EXAMPLES_FOC_HAVE_TORQ
bool "FOC example torque controller support"
default n
+if EXAMPLES_FOC_HAVE_TORQ
+
+config EXAMPLES_FOC_TORQ_MAX
+ int "FOC example torque maximum (x1000)"
+ default 1000
+ ---help---
+ The unit is mini-ampere (1/1000 ampere) for current mode.
+ The unit is mini-volt (1/1000 voltage) for voltage mode.
+
+endif # EXAMPLES_FOC_HAVE_TORQ
+
config EXAMPLES_FOC_HAVE_VEL
bool "FOC example velocity controller support"
default EXAMPLES_FOC_SENSORLESS
@@ -240,12 +251,6 @@ config EXAMPLES_FOC_VELCTRL_PI_KI
The Ki coefficient used in controller is:
Ki = EXAMPLES_FOC_VELCTRL_PI_KI/1000000
-config EXAMPLES_FOC_VELCTRL_PI_SAT
- int "FOC velocity PI saturation (1000x)"
- default 0
- ---help---
- The unit is micro-ampere (1/1000000 ampere)
-
endif # EXAMPLES_FOC_VELCTRL_PI
endif # EXAMPLES_FOC_HAVE_VEL
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index f5bcf8b42..311f259ab 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -932,6 +932,11 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
+ /* Saturate torque */
+
+ f_saturate_b16(&motor->torq.des, -motor->torq_sat,
+ motor->torq_sat);
+
/* Torque setpoint */
motor->torq.set = motor->torq.des;
@@ -1333,6 +1338,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
motor->ol_thr = ftob16(motor->envp->cfg->ol_thr / 1.0f);
motor->ol_hys = ftob16(motor->envp->cfg->ol_hys / 1.0f);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq_sat = ftob16(CONFIG_EXAMPLES_FOC_TORQ_MAX / 1000.0f);
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel_sat = ftob16(CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 407c042e5..b69cdb9e4 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -133,6 +133,7 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
struct foc_setpoint_b16_s torq; /* Torque setpoint */
+ b16_t torq_sat; /* Torque saturation */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_setpoint_b16_s vel; /* Velocity setpoint */
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 12d8ae229..9a9af64df 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -915,6 +915,11 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
+ /* Saturate torque */
+
+ f_saturate(&motor->torq.des, -motor->torq_sat,
+ motor->torq_sat);
+
/* Torque setpoint */
motor->torq.set = motor->torq.des;
@@ -1322,6 +1327,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
motor->ol_thr = (motor->envp->cfg->ol_thr / 1.0f);
motor->ol_hys = (motor->envp->cfg->ol_hys / 1.0f);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq_sat = (CONFIG_EXAMPLES_FOC_TORQ_MAX / 1000.0f);
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel_sat = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index e22758698..f73f10c79 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -133,6 +133,7 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
struct foc_setpoint_f32_s torq; /* Torque setpoint */
+ float torq_sat; /* Torque saturation */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_setpoint_f32_s vel; /* Velocity setpoint */
