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:08 UTC
[incubator-nuttx-apps] 04/06: foc/examples: initial logic to
support torque, velocity and position controller modes
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 b30f3329f669480377186807eb5308c0fa760bc7
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 20:30:37 2021 +0100
foc/examples: initial logic to support torque, velocity and position controller modes
---
examples/foc/Kconfig | 30 ++++-
examples/foc/foc_cfg.h | 11 ++
examples/foc/foc_fixed16_thr.c | 12 +-
examples/foc/foc_float_thr.c | 18 ++-
examples/foc/foc_main.c | 79 ++++++++++--
examples/foc/foc_motor_b16.c | 275 ++++++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_b16.h | 21 +++-
examples/foc/foc_motor_f32.c | 261 +++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_f32.h | 21 +++-
examples/foc/foc_parseargs.c | 59 ++++++++-
examples/foc/foc_parseargs.h | 13 +-
examples/foc/foc_thr.h | 41 ++++--
12 files changed, 731 insertions(+), 110 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index c3cf592..3c1e2c7 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -93,6 +93,19 @@ config EXAMPLES_FOC_HAVE_OPENLOOP
default y if EXAMPLES_FOC_SENSORLESS
default n
+config EXAMPLES_FOC_HAVE_TORQ
+ bool "FOC example torque controller support"
+ default n
+
+config EXAMPLES_FOC_HAVE_VEL
+ bool "FOC example velocity controller support"
+ default y if EXAMPLES_FOC_SENSORLESS
+ default n
+
+config EXAMPLES_FOC_HAVE_POS
+ bool "FOC example position controller support"
+ default n
+
menu "FOC user input"
config EXAMPLES_FOC_HAVE_ADC
@@ -207,14 +220,23 @@ endmenu # FOC user input
menu "FOC controller parameters"
-config EXAMPLES_FOC_OPMODE
- int "FOC operation mode"
+config EXAMPLES_FOC_FMODE
+ int "FOC control mode"
default 2
range 1 3
---help---
1 - IDLE mode
- 2 - voltage open-loop velocity controller (default)
- 3 - current open-loop velocity controller
+ 2 - voltage mode (default)
+ 3 - current mode
+
+config EXAMPLES_FOC_MMODE
+ int "Motor control mode"
+ default 2
+ range 1 3
+ ---help---
+ 1 - torque control
+ 2 - velocity control
+ 3 - position control
config EXAMPLES_FOC_OPENLOOP_Q
int "FOC open-loop Vq/Iq setting [x1000]"
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index a55902d..c67107b 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -37,9 +37,20 @@
# error For now only open-loop supported
#endif
+/* For now only sensorless velocity control supported */
+
+#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
+# ifndef CONFIG_EXAMPLES_FOC_HAVE_VEL
+# error
+# endif
+#endif
+
/* Open-loop configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+# ifndef CONFIG_EXAMPLES_FOC_HAVE_VEL
+# error
+# endif
# ifndef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
# error
# endif
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 9b6a7fd..67f31ec 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -80,9 +80,17 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
motor->angle_now = 0;
motor->vbus = 0;
- /* Force velocity to zero */
+ /* Force setpoints to zero */
- motor->vel_des = 0;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq.des = 0;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ motor->vel.des = 0;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ motor->pos.des = 0;
+#endif
}
/* Get real currents */
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 446290c..6158992 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -75,14 +75,22 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
{
/* Stop motor */
- motor->dq_ref.q = 0;
- motor->dq_ref.d = 0;
- motor->angle_now = 0;
- motor->vbus = 0;
+ motor->dq_ref.q = 0.0f;
+ motor->dq_ref.d = 0.0f;
+ motor->angle_now = 0.0f;
+ motor->vbus = 0.0f;
/* Force velocity to zero */
- motor->vel_des = 0;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq.des = 0.0f;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ motor->vel.des = 0.0f;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ motor->pos.des = 0.0f;
+#endif
}
/* Get real currents */
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 61f7062..c3ddbd8 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -129,8 +129,10 @@ static void init_args(FAR struct args_s *args)
{
args->time =
(args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time);
- args->mode =
- (args->mode == 0 ? CONFIG_EXAMPLES_FOC_OPMODE : args->mode);
+ args->fmode =
+ (args->fmode == 0 ? CONFIG_EXAMPLES_FOC_FMODE : args->fmode);
+ args->mmode =
+ (args->mmode == 0 ? CONFIG_EXAMPLES_FOC_MMODE : args->mmode);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
args->qparam =
(args->qparam == 0 ? CONFIG_EXAMPLES_FOC_OPENLOOP_Q : args->qparam);
@@ -140,8 +142,20 @@ static void init_args(FAR struct args_s *args)
args->pi_ki =
(args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->pi_ki);
- /* For now only velocity control supported */
+ /* Setpoint configuration */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+ args->torqmax =
+ (args->torqmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->torqmax);
+#else
+ args->torqmax =
+ (args->torqmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->torqmax);
+#endif
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
args->velmax =
(args->velmax == 0 ?
@@ -151,6 +165,18 @@ static void init_args(FAR struct args_s *args)
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
#endif
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+ args->posmax =
+ (args->posmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->posmax);
+#else
+ args->posmax =
+ (args->posmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->posmax);
+#endif
+#endif
args->state =
(args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
@@ -173,13 +199,31 @@ static int validate_args(FAR struct args_s *args)
goto errout;
}
- /* Operation mode */
+ /* FOC operation mode */
+
+ if (args->fmode != FOC_FMODE_IDLE &&
+ args->fmode != FOC_FMODE_VOLTAGE &&
+ args->fmode != FOC_FMODE_CURRENT)
+ {
+ PRINTF("Invalid op mode value %d s\n", args->fmode);
+ goto errout;
+ }
+
+ /* Example control mode */
- if (args->mode != FOC_OPMODE_IDLE &&
- args->mode != FOC_OPMODE_OL_V_VEL &&
- args->mode != FOC_OPMODE_OL_C_VEL)
+ if (
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ args->mmode != FOC_MMODE_TORQ &&
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ args->mmode != FOC_MMODE_VEL &&
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ args->mmode != FOC_MMODE_POS &&
+#endif
+ 1)
{
- PRINTF("Invalid op mode value %d s\n", args->mode);
+ PRINTF("Invalid ctrl mode value %d s\n", args->mmode);
goto errout;
}
@@ -598,12 +642,21 @@ int main(int argc, char *argv[])
/* Get configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc[i].qparam = args.qparam;
+ foc[i].qparam = args.qparam;
+#endif
+ foc[i].fmode = args.fmode;
+ foc[i].mmode = args.mmode;
+ foc[i].pi_kp = args.pi_kp;
+ foc[i].pi_ki = args.pi_ki;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ foc[i].torqmax = args.torqmax;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ foc[i].velmax = args.velmax;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ foc[i].posmax = args.posmax;
#endif
- foc[i].mode = args.mode;
- foc[i].pi_kp = args.pi_kp;
- foc[i].pi_ki = args.pi_ki;
- foc[i].velmax = args.velmax;
if (args.en & (1 << i))
{
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 41d03e2..e670a7e 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -113,6 +113,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Initialize velocity ramp */
ret = foc_ramp_init_b16(&motor->ramp,
@@ -125,6 +126,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
goto errout;
}
+#endif
/* Initialize FOC handler */
@@ -203,6 +205,32 @@ static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus)
return OK;
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+/****************************************************************************
+ * Name: foc_motor_torq
+ ****************************************************************************/
+
+static int foc_motor_torq(FAR struct foc_motor_b16_s *motor, uint32_t torq)
+{
+ b16_t tmp1 = 0;
+ b16_t tmp2 = 0;
+
+ DEBUGASSERT(motor);
+
+ /* Update motor torqocity destination
+ * NOTE: torqmax may not fit in b16_t so we can't use b16idiv()
+ */
+
+ tmp1 = itob16(motor->envp->torqmax / 1000);
+ tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
+
+ motor->torq.des = b16muli(tmp2, torq);
+
+ return OK;
+}
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/****************************************************************************
* Name: foc_motor_vel
****************************************************************************/
@@ -221,27 +249,116 @@ static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel)
tmp1 = itob16(motor->envp->velmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
- motor->vel_des = b16muli(tmp2, vel);
+ motor->vel.des = b16muli(tmp2, vel);
return OK;
}
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
/****************************************************************************
- * Name: foc_motor_state
+ * Name: foc_motor_pos
****************************************************************************/
-static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
+static int foc_motor_pos(FAR struct foc_motor_b16_s *motor, uint32_t pos)
{
- int ret = OK;
+ b16_t tmp1 = 0;
+ b16_t tmp2 = 0;
DEBUGASSERT(motor);
- /* Get open-loop currents
- * NOTE: Id always set to 0
+ /* Update motor posocity destination
+ * NOTE: posmax may not fit in b16_t so we can't use b16idiv()
*/
- motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000);
- motor->dq_ref.d = 0;
+ tmp1 = itob16(motor->envp->posmax / 1000);
+ tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
+
+ motor->pos.des = b16muli(tmp2, pos);
+
+ return OK;
+}
+#endif
+
+/****************************************************************************
+ * Name: foc_motor_setpoint
+ ****************************************************************************/
+
+static int foc_motor_setpoint(FAR struct foc_motor_b16_s *motor, uint32_t sp)
+{
+ int ret = OK;
+
+ switch (motor->envp->mmode)
+ {
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ case FOC_MMODE_TORQ:
+ {
+ /* Update motor torque destination */
+
+ ret = foc_motor_torq(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_torq failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ case FOC_MMODE_VEL:
+ {
+ /* Update motor velocity destination */
+
+ ret = foc_motor_vel(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ case FOC_MMODE_POS:
+ {
+ /* Update motor position destination */
+
+ ret = foc_motor_pos(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_pos failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+ default:
+ {
+ PRINTF("ERROR: unsupported ctrl mode %d\n", motor->envp->mmode);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_state
+ ****************************************************************************/
+
+static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
/* Update motor state */
@@ -249,8 +366,7 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
{
case FOC_EXAMPLE_STATE_FREE:
{
- motor->vel_set = 0;
- motor->dir = DIR_NONE_B16;
+ motor->dir = DIR_NONE_B16;
/* Force DQ vector to zero */
@@ -262,8 +378,7 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
case FOC_EXAMPLE_STATE_STOP:
{
- motor->vel_set = 0;
- motor->dir = DIR_NONE_B16;
+ motor->dir = DIR_NONE_B16;
/* DQ vector not zero - active brake */
@@ -272,16 +387,14 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
case FOC_EXAMPLE_STATE_CW:
{
- motor->vel_set = 0;
- motor->dir = DIR_CW_B16;
+ motor->dir = DIR_CW_B16;
break;
}
case FOC_EXAMPLE_STATE_CCW:
{
- motor->vel_set = 0;
- motor->dir = DIR_CCW_B16;
+ motor->dir = DIR_CCW_B16;
break;
}
@@ -293,6 +406,18 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
}
}
+ /* Reset current setpoint */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq.set = 0;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ motor->vel.set = 0;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ motor->pos.set = 0;
+#endif
+
errout:
return ret;
}
@@ -346,23 +471,98 @@ errout:
static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
{
- int ret = OK;
+ b16_t q_ref = 0;
+ b16_t d_ref = 0;
+ int ret = OK;
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
+ DEBUGASSERT(motor);
- motor->vel_now = motor->vel_set;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ /* Open-loop works only in velocity control mode */
- /* Run velocity ramp controller */
+ if (motor->openloop_now == true)
+ {
+ if (motor->envp->mmode != FOC_MMODE_VEL)
+ {
+ PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+# else
+# error
+# endif
+#endif
- ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
+ /* Get previous DQ */
+
+ q_ref = motor->dq_ref.q;
+ d_ref = motor->dq_ref.d;
+
+ /* Controller */
+
+ switch (motor->envp->mmode)
{
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ case FOC_MMODE_TORQ:
+ {
+ motor->torq.set = b16mulb16(motor->dir, motor->torq.des);
+
+ q_ref = motor->torq.set;
+ d_ref = 0;
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ case FOC_MMODE_VEL:
+ {
+ /* 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;
+ }
+
+ break;
+ }
+#endif
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Force open-loop current */
+
+ if (motor->openloop_now == true)
+ {
+ /* Get open-loop currents
+ * NOTE: Id always set to 0
+ */
+
+ motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000);
+ motor->dq_ref.d = 0;
}
+#endif
+
+ /* Set DQ reference frame */
+
+ motor->dq_ref.q = q_ref;
+ motor->dq_ref.d = d_ref;
+
+ /* DQ compensation */
+
+ motor->vdq_comp.q = 0;
+ motor->vdq_comp.d = 0;
errout:
return ret;
@@ -471,7 +671,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
/* Update open-loop angle handler */
- ain.vel = motor->vel_set;
+ ain.vel = motor->vel.set;
ain.angle = motor->angle_now;
ain.dir = motor->dir;
@@ -496,6 +696,19 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
ASSERT(0);
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now == true)
+ {
+ /* No velocity feedback - assume that velocity now is velocity set */
+
+ motor->vel.now = motor->vel.set;
+ }
+ else
+#endif
+ {
+ /* TODO: velocity observer or sensor */
+ }
+
return ret;
}
@@ -615,10 +828,12 @@ int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->setpoint, motor->envp->id);
- ret = foc_motor_vel(motor, handle->setpoint);
+ /* Update motor setpoint */
+
+ ret = foc_motor_setpoint(motor, handle->setpoint);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_setpoint failed %d!\n", ret);
goto errout;
}
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 13a4be9..34bcc37 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -46,6 +46,15 @@
* Public Type Definition
****************************************************************************/
+/* FOC setpoint (fixed16) */
+
+struct foc_setpoint_b16_s
+{
+ b16_t set;
+ b16_t now;
+ b16_t des;
+};
+
/* FOC motor data (fixed16) */
struct foc_motor_b16_s
@@ -62,9 +71,15 @@ struct foc_motor_b16_s
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 */
- b16_t vel_now; /* Velocity now */
- b16_t vel_des; /* Velocity destination */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ struct foc_setpoint_b16_s torq; /* Torque setpoint */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ struct foc_setpoint_b16_s vel; /* Velocity setpoint */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ struct foc_setpoint_b16_s pos; /* Position setpoint */
+#endif
b16_t dir; /* Motor's direction */
b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 4a28846..3dd3219 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -113,6 +113,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Initialize velocity ramp */
ret = foc_ramp_init_f32(&motor->ramp,
@@ -125,6 +126,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
goto errout;
}
+#endif
/* Initialize FOC handler */
@@ -203,6 +205,25 @@ static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus)
return OK;
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+/****************************************************************************
+ * Name: foc_motor_torq
+ ****************************************************************************/
+
+static int foc_motor_torq(FAR struct foc_motor_f32_s *motor, uint32_t torq)
+{
+ DEBUGASSERT(motor);
+
+ /* Update motor torque destination */
+
+ motor->torq.des = (torq * SETPOINT_ADC_SCALE *
+ motor->envp->torqmax / 1000.0f);
+
+ return OK;
+}
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/****************************************************************************
* Name: foc_motor_vel
****************************************************************************/
@@ -213,11 +234,100 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
/* Update motor velocity destination */
- motor->vel_des = (vel * SETPOINT_ADC_SCALE *
+ motor->vel.des = (vel * SETPOINT_ADC_SCALE *
motor->envp->velmax / 1000.0f);
return OK;
}
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+/****************************************************************************
+ * Name: foc_motor_pos
+ ****************************************************************************/
+
+static int foc_motor_pos(FAR struct foc_motor_f32_s *motor, uint32_t pos)
+{
+ DEBUGASSERT(motor);
+
+ /* Update motor position destination */
+
+ motor->pos.des = (pos * SETPOINT_ADC_SCALE *
+ motor->envp->posmax / 1000.0f);
+
+ return OK;
+}
+#endif
+
+/****************************************************************************
+ * Name: foc_motor_setpoint
+ ****************************************************************************/
+
+static int foc_motor_setpoint(FAR struct foc_motor_f32_s *motor, uint32_t sp)
+{
+ int ret = OK;
+
+ switch (motor->envp->mmode)
+ {
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ case FOC_MMODE_TORQ:
+ {
+ /* Update motor torque destination */
+
+ ret = foc_motor_torq(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_torq failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ case FOC_MMODE_VEL:
+ {
+ /* Update motor velocity destination */
+
+ ret = foc_motor_vel(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ case FOC_MMODE_POS:
+ {
+ /* Update motor position destination */
+
+ ret = foc_motor_pos(motor, sp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_pos failed %d!\n", ret);
+ goto errout;
+ }
+
+ break;
+ }
+#endif
+
+ default:
+ {
+ PRINTF("ERROR: unsupported ctrl mode %d\n", motor->envp->mmode);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
/****************************************************************************
* Name: foc_motor_state
@@ -229,21 +339,13 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
DEBUGASSERT(motor);
- /* Get open-loop currents
- * NOTE: Id always set to 0
- */
-
- motor->dq_ref.q = (motor->envp->qparam / 1000.0f);
- motor->dq_ref.d = 0.0f;
-
/* Update motor state */
switch (state)
{
case FOC_EXAMPLE_STATE_FREE:
{
- motor->vel_set = 0.0f;
- motor->dir = DIR_NONE;
+ motor->dir = DIR_NONE;
/* Force DQ vector to zero */
@@ -255,8 +357,7 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
case FOC_EXAMPLE_STATE_STOP:
{
- motor->vel_set = 0.0f;
- motor->dir = DIR_NONE;
+ motor->dir = DIR_NONE;
/* DQ vector not zero - active brake */
@@ -265,16 +366,14 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
case FOC_EXAMPLE_STATE_CW:
{
- motor->vel_set = 0.0f;
- motor->dir = DIR_CW;
+ motor->dir = DIR_CW;
break;
}
case FOC_EXAMPLE_STATE_CCW:
{
- motor->vel_set = 0.0f;
- motor->dir = DIR_CCW;
+ motor->dir = DIR_CCW;
break;
}
@@ -286,6 +385,18 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
}
}
+ /* Reset current setpoint */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ motor->torq.set = 0.0f;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ motor->vel.set = 0.0f;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ motor->pos.set = 0.0f;
+#endif
+
errout:
return ret;
}
@@ -346,24 +457,99 @@ errout:
static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
{
- int ret = OK;
+ float q_ref = 0.0f;
+ float d_ref = 0.0f;
+ int ret = OK;
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
+ DEBUGASSERT(motor);
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ /* Open-loop works only in velocity control mode */
+
+ if (motor->openloop_now == true)
+ {
+ if (motor->envp->mmode != FOC_MMODE_VEL)
+ {
+ PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+# else
+# error
+# endif
+#endif
- motor->vel_now = motor->vel_set;
+ /* Get previous DQ */
- /* Run velocity ramp controller */
+ q_ref = motor->dq_ref.q;
+ d_ref = motor->dq_ref.d;
- ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
+ /* Controller */
+
+ switch (motor->envp->mmode)
{
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ case FOC_MMODE_TORQ:
+ {
+ motor->torq.set = motor->dir * motor->torq.des;
+
+ q_ref = motor->torq.set;
+ d_ref = 0.0f;
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ case FOC_MMODE_VEL:
+ {
+ /* 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;
+ }
+
+ break;
+ }
+#endif
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Force open-loop current */
+
+ if (motor->openloop_now == true)
+ {
+ /* Get open-loop currents
+ * NOTE: Id always set to 0
+ */
+
+ q_ref = (motor->envp->qparam / 1000.0f);
+ d_ref = 0.0f;
+ }
+#endif
+
+ /* Set DQ reference frame */
+
+ motor->dq_ref.q = q_ref;
+ motor->dq_ref.d = d_ref;
+
+ /* DQ compensation */
+
+ motor->vdq_comp.q = 0.0f;
+ motor->vdq_comp.d = 0.0f;
+
errout:
return ret;
}
@@ -471,7 +657,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
/* Update open-loop angle handler */
- ain.vel = motor->vel_set;
+ ain.vel = motor->vel.set;
ain.angle = motor->angle_now;
ain.dir = motor->dir;
@@ -496,6 +682,19 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
ASSERT(0);
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now == true)
+ {
+ /* No velocity feedback - assume that velocity now is velocity set */
+
+ motor->vel.now = motor->vel.set;
+ }
+ else
+#endif
+ {
+ /* TODO: velocity observer or sensor */
+ }
+
return ret;
}
@@ -617,10 +816,12 @@ int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->setpoint, motor->envp->id);
- ret = foc_motor_vel(motor, handle->setpoint);
+ /* Update motor setpoint */
+
+ ret = foc_motor_setpoint(motor, handle->setpoint);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_setpoint failed %d!\n", ret);
goto errout;
}
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index 49043ec..f6e2c0b 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -47,6 +47,15 @@
* Public Type Definition
****************************************************************************/
+/* FOC setpoint (float32) */
+
+struct foc_setpoint_f32_s
+{
+ float set;
+ float now;
+ float des;
+};
+
/* FOC motor data (float32) */
struct foc_motor_f32_s
@@ -63,9 +72,15 @@ struct foc_motor_f32_s
int ctrl_state; /* Controller state */
float vbus; /* Power bus voltage */
float angle_now; /* Phase angle now */
- float vel_set; /* Velocity setting now */
- float vel_now; /* Velocity now */
- float vel_des; /* Velocity destination */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ struct foc_setpoint_f32_s torq; /* Torque setpoint */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ struct foc_setpoint_f32_s vel; /* Velocity setpoint */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ struct foc_setpoint_f32_s pos; /* Position setpoint */
+#endif
float dir; /* Motor's direction */
float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */
diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c
index c945e83..3778bac 100644
--- a/examples/foc/foc_parseargs.c
+++ b/examples/foc/foc_parseargs.c
@@ -46,11 +46,22 @@ static struct option g_long_options[] =
{
{ "time", required_argument, 0, 't' },
{ "help", no_argument, 0, 'h' },
- { "mode", required_argument, 0, 'm' },
+ { "fmode", required_argument, 0, 'f' },
+ { "mmode", required_argument, 0, 'm' },
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ { "torq", required_argument, 0, 'r' },
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
{ "vel", required_argument, 0, 'v' },
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ { "pos", required_argument, 0, 'x' },
+#endif
{ "state", required_argument, 0, 's' },
{ "en", required_argument, 0, 'j' },
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
{ "oqset", required_argument, 0, 'o' },
+#endif
{ "fkp", required_argument, 0, OPT_FKP },
{ "fki", required_argument, 0, OPT_FKI },
{ 0, 0, 0, 0 }
@@ -69,11 +80,23 @@ static void foc_help(void)
PRINTF("Usage: foc [OPTIONS]\n");
PRINTF(" [-t] run time\n");
PRINTF(" [-h] shows this message and exits\n");
- PRINTF(" [-m] controller mode\n");
+ PRINTF(" [-m] operation mode\n");
PRINTF(" 1 - IDLE mode\n");
- PRINTF(" 2 - voltage open-loop velocity \n");
- PRINTF(" 3 - current open-loop velocity \n");
+ PRINTF(" 2 - voltage mode \n");
+ PRINTF(" 3 - current mode \n");
+ PRINTF(" [-c] controller mode\n");
+ PRINTF(" 1 - torqe control \n");
+ PRINTF(" 2 - velocity control \n");
+ PRINTF(" 3 - position control \n");
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ PRINTF(" [-r] torque [x1000]\n");
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
PRINTF(" [-v] velocity [x1000]\n");
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ PRINTF(" [-x] position [x1000]\n");
+#endif
PRINTF(" [-s] motor state\n");
PRINTF(" 1 - motor free\n");
PRINTF(" 2 - motor stop\n");
@@ -102,7 +125,7 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
while (1)
{
- c = getopt_long(argc, argv, "ht:m:o:v:s:j:", g_long_options,
+ c = getopt_long(argc, argv, "ht:f:m:o:r:v:x:s:j:", g_long_options,
&option_index);
if (c == -1)
@@ -136,17 +159,41 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
exit(0);
}
+ case 'f':
+ {
+ args->fmode = atoi(optarg);
+ break;
+ }
+
case 'm':
{
- args->mode = atoi(optarg);
+ args->mmode = atoi(optarg);
break;
}
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ case 'r':
+ {
+ args->torqmax = atoi(optarg);
+ break;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case 'v':
{
args->velmax = atoi(optarg);
break;
}
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ case 'x':
+ {
+ args->posmax = atoi(optarg);
+ break;
+ }
+#endif
case 's':
{
diff --git a/examples/foc/foc_parseargs.h b/examples/foc/foc_parseargs.h
index ecbc6fe..c783356 100644
--- a/examples/foc/foc_parseargs.h
+++ b/examples/foc/foc_parseargs.h
@@ -42,15 +42,24 @@
struct args_s
{
int time; /* Run time limit in sec, -1 if forever */
- int mode; /* Operation mode */
+ int fmode; /* FOC control mode */
+ int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
int qparam; /* Open-loop Q setting (x1000) */
#endif
uint32_t pi_kp; /* PI Kp (x1000) */
uint32_t pi_ki; /* PI Ki (x1000) */
- uint32_t velmax; /* Velocity max (x1000) */
int state; /* Example state (FREE, CW, CCW, STOP) */
int8_t en; /* Enabled instances (bit-encoded) */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ uint32_t torqmax; /* Torque max (x1000) */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ uint32_t velmax; /* Velocity max (x1000) */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ uint32_t posmax; /* Position max (x1000) */
+#endif
};
/****************************************************************************
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index 59fdaa9..c824f68 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -48,21 +48,29 @@ enum foc_example_state_e
FOC_EXAMPLE_STATE_CCW = 4, /* CCW direction */
};
-/* Operation modes */
+/* FOC control mode */
-enum foc_operation_mode_e
+enum foc_foc_mode_e
{
- FOC_OPMODE_INVALID = 0, /* Reserved */
- FOC_OPMODE_IDLE = 1, /* IDLE */
- FOC_OPMODE_OL_V_VEL = 2, /* Voltage open-loop velocity controller */
- FOC_OPMODE_OL_C_VEL = 3, /* Current open-loop velocity controller */
+ FOC_FMODE_INVALID = 0, /* Reserved */
+ FOC_FMODE_IDLE = 1, /* IDLE */
+ FOC_FMODE_VOLTAGE = 2, /* Voltage mode */
+ FOC_FMODE_CURRENT = 3, /* Current mode */
+};
- /* Not supported yet */
+/* Motor control mode */
-#if 0
- FOC_OPMODE_CL_C_TRQ = 3, /* Current closed-loop torque controller */
- FOC_OPMODE_CL_C_VEL = 4, /* Current closed-loop velocity controller */
- FOC_OPMODE_CL_C_POS = 5 /* Current closed-loop position controller */
+enum foc_motor_mode_e
+{
+ FOC_MMODE_INVALID = 0, /* Reserved */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ FOC_MMODE_TORQ = 1, /* Torque control */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ FOC_MMODE_VEL = 2, /* Velocity control */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ FOC_MMODE_POS = 3 /* Position control */
#endif
};
@@ -85,13 +93,22 @@ struct foc_ctrl_env_s
int id; /* FOC device id */
int inst; /* Type specific instance counter */
int type; /* Controller type */
+ int fmode; /* FOC control mode */
+ int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
int qparam; /* Open-loop Q setting (x1000) */
#endif
- int mode; /* Operation mode */
uint32_t pi_kp; /* FOC PI Kp (x1000) */
uint32_t pi_ki; /* FOC PI Ki (x1000) */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+ uint32_t torqmax; /* Torque max (x1000) */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
uint32_t velmax; /* Velocity max (x1000) */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
+ uint32_t posmax; /* Position max (x1000) */
+#endif
};
/****************************************************************************