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

examples/foc: pass from velocity controller to torque controller

Browse Source
This commit is contained in:
raiden00pl 2023-11-09 12:43:46 +01:00 committed by Xiang Xiao
parent ce53f0fc4c
commit 9eb5bd4091
4 changed files with 87 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/foc/Kconfig
View File

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

14
examples/foc/foc_cfg.h
View File

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

61
examples/foc/foc_motor_b16.c
View File

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

60
examples/foc/foc_motor_f32.c
View File

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