This is an automated email from the ASF dual-hosted git repository. xiaoxiang pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git
commit b0eeefd0a5a2af41f35acf2143300df3c49f1537 Author: raiden00pl <[email protected]> AuthorDate: Sun Oct 31 18:40:34 2021 +0100 examples/foc: add logic for controller state machine --- examples/foc/foc_fixed16_thr.c | 53 --------------- examples/foc/foc_float_thr.c | 53 --------------- examples/foc/foc_motor_b16.c | 147 ++++++++++++++++++++++++++++++++++++++--- examples/foc/foc_motor_b16.h | 1 + examples/foc/foc_motor_f32.c | 147 ++++++++++++++++++++++++++++++++++++++--- examples/foc/foc_motor_f32.h | 1 + examples/foc/foc_thr.h | 11 +++ 7 files changed, 285 insertions(+), 128 deletions(-) diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index ce816bb..9b6a7fd 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -54,50 +54,6 @@ ****************************************************************************/ /**************************************************************************** - * 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 ****************************************************************************/ @@ -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); - /* 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 */ handle.app_state = FOC_EXAMPLE_STATE_FREE; diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index 0db14be..446290c 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -54,50 +54,6 @@ ****************************************************************************/ /**************************************************************************** - * 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 ****************************************************************************/ @@ -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); - /* 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 */ handle.app_state = FOC_EXAMPLE_STATE_FREE; diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index ab543f2..41d03e2 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -45,6 +45,56 @@ ****************************************************************************/ /**************************************************************************** + * 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 ****************************************************************************/ @@ -291,6 +341,34 @@ errout: } /**************************************************************************** + * 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 ****************************************************************************/ @@ -334,6 +412,10 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, foc_angle_cfg_b16(&motor->openloop, &ol_cfg); #endif + /* Initialize controller state */ + + motor->ctrl_state = FOC_CTRL_STATE_INIT; + return ret; } @@ -427,20 +509,63 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor) DEBUGASSERT(motor); - /* No velocity feedback - assume that velocity now is velocity set - * TODO: velocity observer or sensor - */ + /* Controller state machine */ - motor->vel_now = motor->vel_set; + switch (motor->ctrl_state) + { + case FOC_CTRL_STATE_INIT: + { + /* Next state */ - /* Run velocity ramp controller */ + motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; - 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; + 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: diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index 0be4aa8..13a4be9 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -59,6 +59,7 @@ struct foc_motor_b16_s foc_angle_b16_t openloop; /* Open-loop angle handler */ #endif int foc_mode; /* FOC mode */ + int ctrl_state; /* Controller state */ b16_t vbus; /* Power bus voltage */ b16_t angle_now; /* Phase angle now */ b16_t vel_set; /* Velocity setting now */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 3c9536b..4a28846 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -45,6 +45,56 @@ ****************************************************************************/ /**************************************************************************** + * 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 ****************************************************************************/ @@ -291,6 +341,34 @@ errout: } /**************************************************************************** + * 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 ****************************************************************************/ @@ -334,6 +412,10 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, foc_angle_cfg_f32(&motor->openloop, &ol_cfg); #endif + /* Initialize controller state */ + + motor->ctrl_state = FOC_CTRL_STATE_INIT; + return ret; } @@ -427,20 +509,63 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor) DEBUGASSERT(motor); - /* No velocity feedback - assume that velocity now is velocity set - * TODO: velocity observer or sensor - */ + /* Controller state machine */ - motor->vel_now = motor->vel_set; + switch (motor->ctrl_state) + { + case FOC_CTRL_STATE_INIT: + { + /* Next state */ - /* Run velocity ramp controller */ + motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; - 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; + 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: diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 1ab5f9c..49043ec 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -60,6 +60,7 @@ struct foc_motor_f32_s foc_angle_f32_t openloop; /* Open-loop angle handler */ #endif int foc_mode; /* FOC mode */ + int ctrl_state; /* Controller state */ float vbus; /* Power bus voltage */ float angle_now; /* Phase angle now */ float vel_set; /* Velocity setting now */ diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h index a714a3d..59fdaa9 100644 --- a/examples/foc/foc_thr.h +++ b/examples/foc/foc_thr.h @@ -66,6 +66,17 @@ enum foc_operation_mode_e #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 */ struct foc_ctrl_env_s
