From b0eeefd0a5a2af41f35acf2143300df3c49f1537 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 31 Oct 2021 18:40:34 +0100 Subject: [PATCH] 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 | 149 ++++++++++++++++++++++++++++++--- examples/foc/foc_motor_b16.h | 1 + examples/foc/foc_motor_f32.c | 149 ++++++++++++++++++++++++++++++--- examples/foc/foc_motor_f32.h | 1 + examples/foc/foc_thr.h | 11 +++ 7 files changed, 287 insertions(+), 130 deletions(-) diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index ce816bb3c..9b6a7fd34 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -53,50 +53,6 @@ * 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 ****************************************************************************/ @@ -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 0db14bec9..446290c20 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -53,50 +53,6 @@ * 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 ****************************************************************************/ @@ -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 ab543f243..41d03e251 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -44,6 +44,56 @@ * 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 ****************************************************************************/ @@ -290,6 +340,34 @@ errout: 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 ****************************************************************************/ @@ -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; - - /* Run velocity ramp controller */ - - ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) + switch (motor->ctrl_state) { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; + case FOC_CTRL_STATE_INIT: + { + /* 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: diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index 0be4aa824..13a4be978 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 3c9536b92..4a2884698 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -44,6 +44,56 @@ * 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 ****************************************************************************/ @@ -290,6 +340,34 @@ errout: 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 ****************************************************************************/ @@ -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; - - /* Run velocity ramp controller */ - - ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) + switch (motor->ctrl_state) { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; + case FOC_CTRL_STATE_INIT: + { + /* 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: diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 1ab5f9c4f..49043ecc0 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 a714a3d59..59fdaa908 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