sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

examples/foc: add velocity PI controller

Browse Source
This commit is contained in:
raiden00pl 2023-05-16 14:10:18 +02:00 committed by Xiang Xiao
parent 9dde6983e6
commit de6a384668
10 changed files with 211 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
examples/foc/Kconfig
View File

@ -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

9
examples/foc/foc_cfg.h
View File

@ -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 */

10
examples/foc/foc_fixed16_thr.c
View File

@ -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:

10
examples/foc/foc_float_thr.c
View File

@ -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:

4
examples/foc/foc_main.c
View File

@ -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
}
};

66
examples/foc/foc_motor_b16.c
View File

@ -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 */

4
examples/foc/foc_motor_b16.h
View File

@ -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 ************************************************************/

66
examples/foc/foc_motor_f32.c
View File

@ -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 */

4
examples/foc/foc_motor_f32.h
View File

@ -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 ************************************************************/

27
examples/foc/foc_parseargs.c
View File

@ -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);