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

examples/foc: add torque saturation

Browse Source
This commit is contained in:
raiden00pl 2023-11-09 12:22:31 +01:00 committed by Xiang Xiao
parent dc4330476d
commit ce53f0fc4c
5 changed files with 29 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/foc/Kconfig
View File

@ -187,6 +187,17 @@ config EXAMPLES_FOC_HAVE_TORQ
bool "FOC example torque controller support" bool "FOC example torque controller support"
default n 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 config EXAMPLES_FOC_HAVE_VEL
bool "FOC example velocity controller support" bool "FOC example velocity controller support"
default EXAMPLES_FOC_SENSORLESS default EXAMPLES_FOC_SENSORLESS
@ -240,12 +251,6 @@ config EXAMPLES_FOC_VELCTRL_PI_KI
The Ki coefficient used in controller is: The Ki coefficient used in controller is:
Ki = EXAMPLES_FOC_VELCTRL_PI_KI/1000000 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_VELCTRL_PI
endif # EXAMPLES_FOC_HAVE_VEL endif # EXAMPLES_FOC_HAVE_VEL

8
examples/foc/foc_motor_b16.c
View File

@ -932,6 +932,11 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ: case FOC_MMODE_TORQ:
{ {
/* Saturate torque */
f_saturate_b16(&motor->torq.des, -motor->torq_sat,
motor->torq_sat);
/* Torque setpoint */ /* Torque setpoint */
motor->torq.set = motor->torq.des; 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_thr = ftob16(motor->envp->cfg->ol_thr / 1.0f);
motor->ol_hys = ftob16(motor->envp->cfg->ol_hys / 1.0f); motor->ol_hys = ftob16(motor->envp->cfg->ol_hys / 1.0f);
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel_sat = ftob16(CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f); motor->vel_sat = ftob16(CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif #endif

1
examples/foc/foc_motor_b16.h
View File

@ -133,6 +133,7 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
struct foc_setpoint_b16_s torq; /* Torque setpoint */ struct foc_setpoint_b16_s torq; /* Torque setpoint */
b16_t torq_sat; /* Torque saturation */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_setpoint_b16_s vel; /* Velocity setpoint */ struct foc_setpoint_b16_s vel; /* Velocity setpoint */

8
examples/foc/foc_motor_f32.c
View File

@ -915,6 +915,11 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ: case FOC_MMODE_TORQ:
{ {
/* Saturate torque */
f_saturate(&motor->torq.des, -motor->torq_sat,
motor->torq_sat);
/* Torque setpoint */ /* Torque setpoint */
motor->torq.set = motor->torq.des; 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_thr = (motor->envp->cfg->ol_thr / 1.0f);
motor->ol_hys = (motor->envp->cfg->ol_hys / 1.0f); motor->ol_hys = (motor->envp->cfg->ol_hys / 1.0f);
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
motor->torq_sat = (CONFIG_EXAMPLES_FOC_TORQ_MAX / 1000.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel_sat = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f); motor->vel_sat = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif #endif

1
examples/foc/foc_motor_f32.h
View File

@ -133,6 +133,7 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
struct foc_setpoint_f32_s torq; /* Torque setpoint */ struct foc_setpoint_f32_s torq; /* Torque setpoint */
float torq_sat; /* Torque saturation */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_setpoint_f32_s vel; /* Velocity setpoint */ struct foc_setpoint_f32_s vel; /* Velocity setpoint */