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

examples/foc: add logic for controller state machine

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 18:40:34 +01:00 committed by Xiang Xiao
parent 5a9b3a70f7
commit b0eeefd0a5
7 changed files with 287 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
examples/foc/foc_fixed16_thr.c
View File

@ -53,50 +53,6 @@
* Private Functions * Private Functions
****************************************************************************/ ****************************************************************************/
/****************************************************************************
* Name: foc_mode_init
****************************************************************************/
static int foc_mode_init(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
switch (motor->envp->mode)
{
case FOC_OPMODE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
case FOC_OPMODE_OL_V_VEL:
{
motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
motor->openloop_now = true;
break;
}
case FOC_OPMODE_OL_C_VEL:
{
motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
motor->openloop_now = true;
break;
}
#endif
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
ret = -EINVAL;
goto errout;
}
}
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_handler_run * Name: foc_handler_run
****************************************************************************/ ****************************************************************************/
@ -254,15 +210,6 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
motor.pwm_duty_max = FOCDUTY_TO_FIXED16(dev.info.hw_cfg.pwm_max); motor.pwm_duty_max = FOCDUTY_TO_FIXED16(dev.info.hw_cfg.pwm_max);
/* Initialize controller mode */
ret = foc_mode_init(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
goto errout;
}
/* Start with motor free */ /* Start with motor free */
handle.app_state = FOC_EXAMPLE_STATE_FREE; handle.app_state = FOC_EXAMPLE_STATE_FREE;

53
examples/foc/foc_float_thr.c
View File

@ -53,50 +53,6 @@
* Private Functions * Private Functions
****************************************************************************/ ****************************************************************************/
/****************************************************************************
* Name: foc_mode_init
****************************************************************************/
static int foc_mode_init(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
switch (motor->envp->mode)
{
case FOC_OPMODE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
case FOC_OPMODE_OL_V_VEL:
{
motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
motor->openloop_now = true;
break;
}
case FOC_OPMODE_OL_C_VEL:
{
motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
motor->openloop_now = true;
break;
}
#endif
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
ret = -EINVAL;
goto errout;
}
}
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_handler_run * Name: foc_handler_run
****************************************************************************/ ****************************************************************************/
@ -255,15 +211,6 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max); motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max);
/* Initialize controller mode */
ret = foc_mode_init(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
goto errout;
}
/* Start with motor free */ /* Start with motor free */
handle.app_state = FOC_EXAMPLE_STATE_FREE; handle.app_state = FOC_EXAMPLE_STATE_FREE;

149
examples/foc/foc_motor_b16.c
View File

@ -44,6 +44,56 @@
* Private Functions * Private Functions
****************************************************************************/ ****************************************************************************/
/****************************************************************************
* Name: foc_runmode_init
****************************************************************************/
static int foc_runmode_init(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
switch (motor->envp->fmode)
{
case FOC_FMODE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_FMODE_VOLTAGE:
{
motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
break;
}
case FOC_FMODE_CURRENT:
{
motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
break;
}
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->fmode);
ret = -EINVAL;
goto errout;
}
}
/* Force open-loop if sensorlesss */
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
motor->openloop_now = true;
# else
# error
# endif
#endif
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_motor_configure * Name: foc_motor_configure
****************************************************************************/ ****************************************************************************/
@ -290,6 +340,34 @@ errout:
return ret; return ret;
} }
/****************************************************************************
* Name: foc_motor_run
****************************************************************************/
static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Public Functions * Public Functions
****************************************************************************/ ****************************************************************************/
@ -334,6 +412,10 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
foc_angle_cfg_b16(&motor->openloop, &ol_cfg); foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
#endif #endif
/* Initialize controller state */
motor->ctrl_state = FOC_CTRL_STATE_INIT;
return ret; return ret;
} }
@ -427,20 +509,63 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor); DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set /* Controller state machine */
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set; switch (motor->ctrl_state)
/* Run velocity ramp controller */
ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{ {
PRINTF("ERROR: foc_ramp_run failed %d\n", ret); case FOC_CTRL_STATE_INIT:
goto errout; {
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_CTRL_STATE_RUN_INIT:
{
/* Initialize run controller mode */
ret = foc_runmode_init(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_runmode_init failed %d!\n", ret);
goto errout;
}
/* Next state */
motor->ctrl_state += 1;
}
case FOC_CTRL_STATE_RUN:
{
/* Run motor */
ret = foc_motor_run(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
goto errout;
}
break;
}
case FOC_CTRL_STATE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
default:
{
PRINTF("ERROR: invalid ctrl_state=%d\n", motor->ctrl_state);
ret = -EINVAL;
goto errout;
}
} }
errout: errout:

1
examples/foc/foc_motor_b16.h
View File

@ -59,6 +59,7 @@ struct foc_motor_b16_s
foc_angle_b16_t openloop; /* Open-loop angle handler */ foc_angle_b16_t openloop; /* Open-loop angle handler */
#endif #endif
int foc_mode; /* FOC mode */ int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
b16_t vbus; /* Power bus voltage */ b16_t vbus; /* Power bus voltage */
b16_t angle_now; /* Phase angle now */ b16_t angle_now; /* Phase angle now */
b16_t vel_set; /* Velocity setting now */ b16_t vel_set; /* Velocity setting now */

149
examples/foc/foc_motor_f32.c
View File

@ -44,6 +44,56 @@
* Private Functions * Private Functions
****************************************************************************/ ****************************************************************************/
/****************************************************************************
* Name: foc_runmode_init
****************************************************************************/
static int foc_runmode_init(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
switch (motor->envp->fmode)
{
case FOC_FMODE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_FMODE_VOLTAGE:
{
motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
break;
}
case FOC_FMODE_CURRENT:
{
motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
break;
}
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->fmode);
ret = -EINVAL;
goto errout;
}
}
/* Force open-loop if sensorless */
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
motor->openloop_now = true;
# else
# error
# endif
#endif
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_motor_configure * Name: foc_motor_configure
****************************************************************************/ ****************************************************************************/
@ -290,6 +340,34 @@ errout:
return ret; return ret;
} }
/****************************************************************************
* Name: foc_motor_run
****************************************************************************/
static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/**************************************************************************** /****************************************************************************
* Public Functions * Public Functions
****************************************************************************/ ****************************************************************************/
@ -334,6 +412,10 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
foc_angle_cfg_f32(&motor->openloop, &ol_cfg); foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
#endif #endif
/* Initialize controller state */
motor->ctrl_state = FOC_CTRL_STATE_INIT;
return ret; return ret;
} }
@ -427,20 +509,63 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor); DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set /* Controller state machine */
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set; switch (motor->ctrl_state)
/* Run velocity ramp controller */
ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{ {
PRINTF("ERROR: foc_ramp_run failed %d\n", ret); case FOC_CTRL_STATE_INIT:
goto errout; {
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_CTRL_STATE_RUN_INIT:
{
/* Initialize run controller mode */
ret = foc_runmode_init(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_runmode_init failed %d!\n", ret);
goto errout;
}
/* Next state */
motor->ctrl_state += 1;
}
case FOC_CTRL_STATE_RUN:
{
/* Run motor */
ret = foc_motor_run(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
goto errout;
}
break;
}
case FOC_CTRL_STATE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
default:
{
PRINTF("ERROR: invalid ctrl_state=%d\n", motor->ctrl_state);
ret = -EINVAL;
goto errout;
}
} }
errout: errout:

1
examples/foc/foc_motor_f32.h
View File

@ -60,6 +60,7 @@ struct foc_motor_f32_s
foc_angle_f32_t openloop; /* Open-loop angle handler */ foc_angle_f32_t openloop; /* Open-loop angle handler */
#endif #endif
int foc_mode; /* FOC mode */ int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
float vbus; /* Power bus voltage */ float vbus; /* Power bus voltage */
float angle_now; /* Phase angle now */ float angle_now; /* Phase angle now */
float vel_set; /* Velocity setting now */ float vel_set; /* Velocity setting now */

11
examples/foc/foc_thr.h
View File

@ -66,6 +66,17 @@ enum foc_operation_mode_e
#endif #endif
}; };
/* Controller state */
enum foc_controller_state_e
{
FOC_CTRL_STATE_INVALID = 0,
FOC_CTRL_STATE_INIT,
FOC_CTRL_STATE_RUN_INIT,
FOC_CTRL_STATE_RUN,
FOC_CTRL_STATE_IDLE
};
/* FOC thread data */ /* FOC thread data */
struct foc_ctrl_env_s struct foc_ctrl_env_s