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 2d034ed09af39b98b01a74ac9f209314a124f76f
Author: raiden00pl <raide...@railab.me>
AuthorDate: Sat Oct 30 14:45:03 2021 +0200
examples/foc: separate motor control logic from motor state logic
---
examples/foc/foc_fixed16_thr.c | 64 ++++++++++++++++++++++++++++--------------
examples/foc/foc_float_thr.c | 64 ++++++++++++++++++++++++++++--------------
2 files changed, 86 insertions(+), 42 deletions(-)
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 6b6fc98..b41f313 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_run
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
+static int foc_motor_get(FAR struct foc_motor_b16_s *motor)
{
struct foc_angle_in_b16_s ain;
struct foc_angle_out_b16_s aout;
@@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
- /* 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;
- }
-
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
ASSERT(0);
}
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_control
+ ****************************************************************************/
+
+static int foc_motor_control(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* 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;
}
@@ -933,12 +946,21 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
}
}
- /* Run motor controller */
+ /* Get motor state */
+
+ ret = foc_motor_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Motor control */
- ret = foc_motor_run(&motor);
+ ret = foc_motor_control(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index e1a7eed..3a5ef25 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_run
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
+static int foc_motor_get(FAR struct foc_motor_f32_s *motor)
{
struct foc_angle_in_f32_s ain;
struct foc_angle_out_f32_s aout;
@@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
- /* 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;
- }
-
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
ASSERT(0);
}
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_control
+ ****************************************************************************/
+
+static int foc_motor_control(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* 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;
}
@@ -935,12 +948,21 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
}
}
- /* Run motor controller */
+ /* Get motor state */
+
+ ret = foc_motor_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Motor control */
- ret = foc_motor_run(&motor);
+ ret = foc_motor_control(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
