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

examples/foc: add velocity saturation

Browse Source
This commit is contained in:
raiden00pl 2023-11-09 12:05:28 +01:00 committed by Xiang Xiao
parent 32a8c29753
commit dc4330476d
5 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/foc/Kconfig
View File

@ -209,6 +209,12 @@ config EXAMPLES_FOC_VELNOW_FILTER
int "FOC example velocity controller (x1000)" int "FOC example velocity controller (x1000)"
default 990 default 990
config EXAMPLES_FOC_VEL_MAX
int "FOC example velocity maximum (x1)"
default 1000
---help---
The unit is rad/s
choice choice
prompt "FOC velocity controller selection" prompt "FOC velocity controller selection"
default EXAMPLES_FOC_VELCTRL_PI default EXAMPLES_FOC_VELCTRL_PI

11
examples/foc/foc_motor_b16.c
View File

@ -26,6 +26,7 @@
#include <assert.h> #include <assert.h>
#include <string.h> #include <string.h>
#include <dspb16.h>
#include "foc_cfg.h" #include "foc_cfg.h"
#include "foc_debug.h" #include "foc_debug.h"
@ -945,6 +946,13 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL: case FOC_MMODE_VEL:
{ {
/* Saturate velocity */
f_saturate_b16(&motor->vel.des, -motor->vel_sat,
motor->vel_sat);
/* Velocity controller */
if (motor->time % VEL_CONTROL_PRESCALER == 0) if (motor->time % VEL_CONTROL_PRESCALER == 0)
{ {
/* Run velocity ramp controller */ /* Run velocity ramp controller */
@ -1325,6 +1333,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_VEL
motor->vel_sat = ftob16(CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */ /* Initialize controller run mode */

1
examples/foc/foc_motor_b16.h
View File

@ -136,6 +136,7 @@ struct foc_motor_b16_s
#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 */
b16_t vel_sat; /* Velocity saturation */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
struct foc_setpoint_b16_s pos; /* Position setpoint */ struct foc_setpoint_b16_s pos; /* Position setpoint */

11
examples/foc/foc_motor_f32.c
View File

@ -26,6 +26,7 @@
#include <assert.h> #include <assert.h>
#include <string.h> #include <string.h>
#include <dsp.h>
#include "foc_cfg.h" #include "foc_cfg.h"
#include "foc_debug.h" #include "foc_debug.h"
@ -928,6 +929,13 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL: case FOC_MMODE_VEL:
{ {
/* Saturate velocity */
f_saturate(&motor->vel.des, -motor->vel_sat,
motor->vel_sat);
/* Velocity controller */
if (motor->time % VEL_CONTROL_PRESCALER == 0) if (motor->time % VEL_CONTROL_PRESCALER == 0)
{ {
/* Run velocity ramp controller */ /* Run velocity ramp controller */
@ -1314,6 +1322,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_VEL
motor->vel_sat = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */ /* Initialize controller run mode */

1
examples/foc/foc_motor_f32.h
View File

@ -136,6 +136,7 @@ struct foc_motor_f32_s
#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 */
float vel_sat; /* Velocity saturation */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
struct foc_setpoint_f32_s pos; /* Position setpoint */ struct foc_setpoint_f32_s pos; /* Position setpoint */