You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2021/11/04 18:51:07 UTC
[incubator-nuttx-apps] 03/06: examples/foc: add logic for
controller state machine
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 <ra...@railab.me>
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