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
The following commit(s) were added to refs/heads/master by this push:
new de6a38466 examples/foc: add velocity PI controller
de6a38466 is described below
commit de6a3846686bfd0fd52e6e1146996100ade7922e
Author: raiden00pl <raide...@railab.me>
AuthorDate: Tue May 16 14:10:18 2023 +0200
examples/foc: add velocity PI controller
---
examples/foc/Kconfig | 37 +++++++++++++++++++++++
examples/foc/foc_cfg.h | 9 ++++++
examples/foc/foc_fixed16_thr.c | 10 +++----
examples/foc/foc_float_thr.c | 10 +++----
examples/foc/foc_main.c | 4 +++
examples/foc/foc_motor_b16.c | 66 +++++++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_b16.h | 4 +++
examples/foc/foc_motor_f32.c | 66 +++++++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_f32.h | 4 +++
examples/foc/foc_parseargs.c | 27 +++++++++++++++++
10 files changed, 211 insertions(+), 26 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 0e4de7207..c3d03f9f8 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -196,10 +196,47 @@ config EXAMPLES_FOC_VELOBS
if EXAMPLES_FOC_HAVE_VEL
+config EXAMPLES_FOC_VELCTRL_FREQ
+ int "FOC example velocity controller frequency"
+ default 1000
+
config EXAMPLES_FOC_VELNOW_FILTER
int "FOC example velocity controller (x1000)"
default 990
+choice
+ prompt "FOC velocity controller selection"
+ default EXAMPLES_FOC_VELCTRL_PI
+
+config EXAMPLES_FOC_VELCTRL_PI
+ bool "FOC velocity PI controller"
+
+endchoice # FOC velocity controller
+
+if EXAMPLES_FOC_VELCTRL_PI
+
+config EXAMPLES_FOC_VELCTRL_PI_KP
+ int "FOC velocity PI Kp (1000000x)"
+ default 0
+ ---help---
+ The Kp coefficient used in controller is:
+ Kp = EXAMPLES_FOC_VELCTRL_PI_KP/1000000
+
+config EXAMPLES_FOC_VELCTRL_PI_KI
+ int "FOC velocity PI Ki (1000000x)"
+ default 0
+ ---help---
+ 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
if EXAMPLES_FOC_VELOBS
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 7a59c7c9d..9475cdbf7 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -199,6 +199,11 @@
# endif
#endif
+/* Velocity controller prescaler */
+
+#define VEL_CONTROL_PRESCALER (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \
+ CONFIG_EXAMPLES_FOC_VELCTRL_FREQ)
+
/****************************************************************************
* Public Type Definition
****************************************************************************/
@@ -246,6 +251,10 @@ struct foc_thr_cfg_s
uint32_t vel_div_samples; /* Vel DIV observer samples */
uint32_t vel_div_filter; /* Vel DIV observer filter (x1000) */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ uint32_t vel_pi_kp; /* Vel controller PI Kp (x1000000) */
+ uint32_t vel_pi_ki; /* Vel controller PI Ki (x1000000) */
+#endif
};
#endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index d3d7bbc5c..1b0c89700 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -295,10 +295,8 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
struct foc_mq_s handle;
struct foc_motor_b16_s motor;
struct foc_device_s dev;
- int time = 0;
int ret = OK;
- UNUSED(time);
DEBUGASSERT(envp);
PRINTFV("foc_fixed_thr, id=%d\n", envp->id);
@@ -341,7 +339,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
while (motor.mq.quit == false)
{
- PRINTFV("foc_fixed16_thr %d %d\n", envp->id, time);
+ PRINTFV("foc_fixed16_thr %d %d\n", envp->id, motor.time);
/* Handle mqueue */
@@ -441,7 +439,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
#ifdef FOC_STATE_PRINT_PRE
/* Print state if configured */
- if (time % FOC_STATE_PRINT_PRE == 0)
+ if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
@@ -467,7 +465,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/* Capture nxscope samples */
- if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
+ if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_fixed16_nxscope(envp->nxs, &motor, &dev);
}
@@ -488,7 +486,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Increase counter */
- time += 1;
+ motor.time += 1;
}
errout:
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 94d8596be..0e2d89d62 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -300,10 +300,8 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
struct foc_mq_s handle;
struct foc_motor_f32_s motor;
struct foc_device_s dev;
- int time = 0;
int ret = OK;
- UNUSED(time);
DEBUGASSERT(envp);
PRINTFV("foc_float_thr, id=%d\n", envp->id);
@@ -346,7 +344,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
while (motor.mq.quit == false)
{
- PRINTFV("foc_float_thr %d %d\n", envp->id, time);
+ PRINTFV("foc_float_thr %d %d\n", envp->id, motor.time);
/* Handle mqueue */
@@ -446,7 +444,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
#ifdef FOC_STATE_PRINT_PRE
/* Print state if configured */
- if (time % FOC_STATE_PRINT_PRE == 0)
+ if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
@@ -455,7 +453,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/* Capture nxscope samples */
- if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
+ if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_float_nxscope(envp->nxs, &motor, &dev);
}
@@ -493,7 +491,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Increase counter */
- time += 1;
+ motor.time += 1;
}
errout:
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 9258a46f6..79107073f 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -119,6 +119,10 @@ struct args_s g_args =
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
.vel_div_samples = CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES,
.vel_div_filter = CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER,
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ .vel_pi_kp = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP,
+ .vel_pi_ki = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI,
#endif
}
};
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index f039c2f5e..68ab8741b 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -777,6 +777,9 @@ static int foc_motor_run_init(FAR struct foc_motor_b16_s
*motor)
static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
{
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ b16_t vel_err = 0.0f;
+#endif
b16_t q_ref = 0;
b16_t d_ref = 0;
int ret = OK;
@@ -804,6 +807,14 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
q_ref = motor->dq_ref.q;
d_ref = motor->dq_ref.d;
+ /* Ignore controller if motor is free or stopped */
+
+ if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE ||
+ motor->mq.app_state == FOC_EXAMPLE_STATE_STOP)
+ {
+ goto no_controller;
+ }
+
/* Controller */
switch (motor->envp->cfg->mmode)
@@ -823,14 +834,39 @@ static int foc_motor_run(FAR struct foc_motor_b16_s
*motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_b16(&motor->ramp, motor->vel.des,
- motor->vel.now, &motor->vel.set);
- if (ret < 0)
+ if (motor->time % VEL_CONTROL_PRESCALER == 0)
{
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_b16(&motor->ramp,
+ motor->dir * motor->vel.des,
+ motor->vel.now,
+ &motor->vel.set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Run velocity controller if no in open-loop */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now == false)
+#endif
+ {
+ /* Get velocity error */
+
+ vel_err = motor->vel.set - motor->vel.now;
+
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* PI velocit controller */
+
+ q_ref = pi_controller_b16(&motor->vel_pi, vel_err);
+ d_ref = 0;
+#else
+# error Missing velocity controller
+#endif
+ }
}
break;
@@ -858,6 +894,8 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
}
#endif
+no_controller:
+
/* Set DQ reference frame */
motor->dq_ref.q = q_ref;
@@ -1067,6 +1105,20 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* Initialize velocity controller */
+
+ pi_controller_init_b16(&motor->vel_pi,
+ 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_antiwindup_enable_b16(&motor->vel_pi, ftob16(0.99f), true);
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 843302da8..a10bf2d93 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -87,6 +87,7 @@ struct foc_motor_b16_s
dq_frame_b16_t dq_ref; /* DQ reference */
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
int foc_mode; /* FOC mode */
+ int time; /* Helper counter */
b16_t vbus; /* Power bus voltage */
b16_t per; /* Controller period in seconds
*/
b16_t iphase_adc; /* Iphase ADC scaling factor */
@@ -95,6 +96,9 @@ struct foc_motor_b16_s
/* Velocity controller data ***********************************************/
struct foc_ramp_b16_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ pid_controller_b16_t vel_pi; /* Velocity controller */
+#endif
/* Motor state ************************************************************/
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 35ef15da5..f55f8f79b 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -761,6 +761,9 @@ static int foc_motor_run_init(FAR struct foc_motor_f32_s
*motor)
static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
{
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ float vel_err = 0.0f;
+#endif
float q_ref = 0.0f;
float d_ref = 0.0f;
int ret = OK;
@@ -788,6 +791,14 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
q_ref = motor->dq_ref.q;
d_ref = motor->dq_ref.d;
+ /* Ignore controller if motor is free or stopped */
+
+ if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE ||
+ motor->mq.app_state == FOC_EXAMPLE_STATE_STOP)
+ {
+ goto no_controller;
+ }
+
/* Controller */
switch (motor->envp->cfg->mmode)
@@ -807,14 +818,39 @@ static int foc_motor_run(FAR struct foc_motor_f32_s
*motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_f32(&motor->ramp, motor->vel.des,
- motor->vel.now, &motor->vel.set);
- if (ret < 0)
+ if (motor->time % VEL_CONTROL_PRESCALER == 0)
{
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_f32(&motor->ramp,
+ motor->dir * motor->vel.des,
+ motor->vel.now,
+ &motor->vel.set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Run velocity controller if no in open-loop */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now == false)
+#endif
+ {
+ /* Get velocity error */
+
+ vel_err = motor->vel.set - motor->vel.now;
+
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* PI velocit controller */
+
+ q_ref = pi_controller(&motor->vel_pi, vel_err);
+ d_ref = 0;
+#else
+# error Missing velocity controller
+#endif
+ }
}
break;
@@ -842,6 +878,8 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
}
#endif
+no_controller:
+
/* Set DQ reference frame */
motor->dq_ref.q = q_ref;
@@ -1051,6 +1089,20 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* Initialize velocity controller */
+
+ pi_controller_init(&motor->vel_pi,
+ (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_antiwindup_enable(&motor->vel_pi, 0.99f, true);
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index 84d8a3c7b..ddcec9fc9 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -87,6 +87,7 @@ struct foc_motor_f32_s
dq_frame_f32_t dq_ref; /* DQ reference */
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
int foc_mode; /* FOC mode */
+ int time; /* Helper counter */
float vbus; /* Power bus voltage */
float per; /* Controller period in seconds
*/
float iphase_adc; /* Iphase ADC scaling factor */
@@ -95,6 +96,9 @@ struct foc_motor_f32_s
/* Velocity controller data ***********************************************/
struct foc_ramp_f32_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ pid_controller_f32_t vel_pi; /* Velocity controller */
+#endif
/* Motor state ************************************************************/
diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c
index 609500eac..b43812164 100644
--- a/examples/foc/foc_parseargs.c
+++ b/examples/foc/foc_parseargs.c
@@ -50,6 +50,9 @@
#define OPT_VODIVS (SCHAR_MAX + 9)
#define OPT_VODIVF (SCHAR_MAX + 10)
+#define OPT_VCPIKP (SCHAR_MAX + 11)
+#define OPT_VCPIKI (SCHAR_MAX + 12)
+
/****************************************************************************
* Private Data
****************************************************************************/
@@ -92,6 +95,10 @@ static struct option g_long_options[] =
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
{ "vodivs", required_argument, 0, OPT_VODIVS },
{ "vodivf", required_argument, 0, OPT_VODIVF },
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ { "vcpikp", required_argument, 0, OPT_VCPIKP },
+ { "vcpiki", required_argument, 0, OPT_VCPIKI },
#endif
{ 0, 0, 0, 0 }
};
@@ -178,6 +185,12 @@ static void foc_help(void)
PRINTF(" [--vodivf] velobs DIV filter (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ PRINTF(" [--vpikp] velctrl PI Kp (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP);
+ PRINTF(" [--vpiki] velctrl PI Ki (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI);
+#endif
}
/****************************************************************************
@@ -279,6 +292,20 @@ void parse_args(FAR struct args_s *args, int argc, FAR
char **argv)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ case OPT_VCPIKP:
+ {
+ args->cfg.vel_pi_kp = atoi(optarg);
+ break;
+ }
+
+ case OPT_VCPIKI:
+ {
+ args->cfg.vel_pi_ki = atoi(optarg);
+ break;
+ }
+#endif
+
case 't':
{
args->time = atoi(optarg);
