You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2023/11/14 17:46:17 UTC
(nuttx-apps) 03/03: examples/foc: pass from velocity controller to torque controller
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 9eb5bd409176ecf1a0ba5e32afceb9a32650ac7d
Author: raiden00pl <ra...@railab.me>
AuthorDate: Thu Nov 9 12:43:46 2023 +0100
examples/foc: pass from velocity controller to torque controller
---
examples/foc/Kconfig | 2 ++
examples/foc/foc_cfg.h | 14 ++++++++++
examples/foc/foc_motor_b16.c | 61 ++++++++++++++++++++++++++------------------
examples/foc/foc_motor_f32.c | 60 +++++++++++++++++++++++++------------------
4 files changed, 87 insertions(+), 50 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index abeee8be2..1e0d8851d 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -201,10 +201,12 @@ endif # EXAMPLES_FOC_HAVE_TORQ
config EXAMPLES_FOC_HAVE_VEL
bool "FOC example velocity controller support"
default EXAMPLES_FOC_SENSORLESS
+ select EXAMPLES_FOC_HAVE_TORQ
config EXAMPLES_FOC_HAVE_POS
bool "FOC example position controller support"
default n
+ select EXAMPLES_FOC_HAVE_VEL
config EXAMPLES_FOC_VELOBS
bool "FOC example velocity observer support"
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 1c7bd1965..1d2de0107 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -46,6 +46,20 @@
# endif
#endif
+/* Position controller needs velocity controller */
+
+#if defined(CONFIG_EXAMPLES_FOC_HAVE_POS) && \
+ !defined(CONFIG_EXAMPLES_FOC_HAVE_VEL)
+# error Position controller needs velocity controller
+#endif
+
+/* Velocity controller needs torque controller */
+
+#if defined(CONFIG_EXAMPLES_FOC_HAVE_VEL) && \
+ !defined(CONFIG_EXAMPLES_FOC_HAVE_TORQ)
+# error Velocity controller needs torque controller
+#endif
+
/* Open-loop configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 311f259ab..251f56272 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -925,29 +925,20 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
goto no_controller;
}
- /* Controller */
+ /* Controller.
+ *
+ * The FOC motor controller is a cascade controller:
+ *
+ * 1. Position controller sets requested velocity,
+ * 2. Velocity controller sets requested torque,
+ * 3. Torque controller sets requested motor phase voltages.
+ *
+ * NOTE: the motor torque is directly proportional to the motor
+ * current which is proportional to the motor set voltage
+ */
switch (motor->envp->cfg->mmode)
{
-#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;
-
- q_ref = motor->torq.set;
- d_ref = 0;
-
- break;
- }
-#endif
-
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
@@ -985,14 +976,31 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
/* PI velocit controller */
- q_ref = pi_controller_b16(&motor->vel_pi, vel_err);
- d_ref = 0;
+ motor->torq.des = pi_controller_b16(&motor->vel_pi,
+ vel_err);
#else
# error Missing velocity controller
#endif
}
}
+ /* Don't break here! pass to torque controller */
+ }
+#endif
+
+#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;
+ motor->torq.now = motor->foc_state.idq.q;
+
break;
}
#endif
@@ -1004,6 +1012,11 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
}
}
+ /* Get dq ref */
+
+ q_ref = motor->torq.set;
+ d_ref = 0;
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */
@@ -1547,9 +1560,7 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
ftob16(motor->envp->cfg->vel_pi_kp / 1000000.0f),
ftob16(motor->envp->cfg->vel_pi_ki / 1000000.0f));
- pi_saturation_set_b16(&motor->vel_pi,
- ftob16(-CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f),
- ftob16(CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f));
+ pi_saturation_set_b16(&motor->vel_pi, -motor->torq_sat, motor->torq_sat);
pi_antiwindup_enable_b16(&motor->vel_pi, ftob16(0.99f), true);
#endif
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 9a9af64df..4a9a34e11 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -908,29 +908,20 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
goto no_controller;
}
- /* Controller */
+ /* Controller.
+ *
+ * The FOC motor controller is a cascade controller:
+ *
+ * 1. Position controller sets requested velocity,
+ * 2. Velocity controller sets requested torque,
+ * 3. Torque controller sets requested motor phase voltages.
+ *
+ * NOTE: the motor torque is directly proportional to the motor
+ * current which is proportional to the motor set voltage
+ */
switch (motor->envp->cfg->mmode)
{
-#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;
-
- q_ref = motor->torq.set;
- d_ref = 0.0f;
-
- break;
- }
-#endif
-
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
@@ -968,14 +959,30 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
/* PI velocit controller */
- q_ref = pi_controller(&motor->vel_pi, vel_err);
- d_ref = 0;
+ motor->torq.des = pi_controller(&motor->vel_pi,
+ vel_err);
#else
# error Missing velocity controller
#endif
}
}
+ /* Don't break here! pass to torque controller */
+ }
+#endif
+
+#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;
+
break;
}
#endif
@@ -987,6 +994,11 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
}
}
+ /* Get dq ref */
+
+ q_ref = motor->torq.set;
+ d_ref = 0.0f;
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */
@@ -1536,9 +1548,7 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
(motor->envp->cfg->vel_pi_kp / 1000000.0f),
(motor->envp->cfg->vel_pi_ki / 1000000.0f));
- pi_saturation_set(&motor->vel_pi,
- -(CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f),
- (CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f));
+ pi_saturation_set(&motor->vel_pi, -motor->torq_sat, motor->torq_sat);
pi_antiwindup_enable(&motor->vel_pi, 0.99f, true);
#endif