diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index c3cf592f6..3c1e2c70a 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 a55902dd3..c67107bf5 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 9b6a7fd34..67f31ecdc 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 446290c20..615899276 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 61f706211..c3ddbd86c 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 ? @@ -150,6 +164,18 @@ static void init_args(FAR struct args_s *args) args->velmax = (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 = @@ -173,13 +199,31 @@ static int validate_args(FAR struct args_s *args) goto errout; } - /* Operation mode */ + /* FOC operation mode */ - if (args->mode != FOC_OPMODE_IDLE && - args->mode != FOC_OPMODE_OL_V_VEL && - args->mode != FOC_OPMODE_OL_C_VEL) + 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->mode); + PRINTF("Invalid op mode value %d s\n", args->fmode); + goto errout; + } + + /* Example control mode */ + + 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 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 41d03e251..e670a7ee0 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,10 +249,106 @@ 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_pos + ****************************************************************************/ + +static int foc_motor_pos(FAR struct foc_motor_b16_s *motor, uint32_t pos) +{ + b16_t tmp1 = 0; + b16_t tmp2 = 0; + + DEBUGASSERT(motor); + + /* Update motor posocity destination + * NOTE: posmax may not fit in b16_t so we can't use b16idiv() + */ + + 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 @@ -236,21 +360,13 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) DEBUGASSERT(motor); - /* Get open-loop currents - * NOTE: Id always set to 0 - */ - - motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000); - motor->dq_ref.d = 0; - /* Update motor state */ switch (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 */ - - ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) + if (motor->openloop_now == true) { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; + 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 + + /* Get previous DQ */ + + q_ref = motor->dq_ref.q; + d_ref = motor->dq_ref.d; + + /* Controller */ + + switch (motor->envp->mmode) + { +#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 13a4be978..34bcc376e 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 4a2884698..3dd3219df 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,23 +457,98 @@ 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); - 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 */ - - ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) + if (motor->openloop_now == true) { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; + 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 + + /* Get previous DQ */ + + q_ref = motor->dq_ref.q; + d_ref = motor->dq_ref.d; + + /* Controller */ + + switch (motor->envp->mmode) + { +#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 49043ecc0..f6e2c0bf8 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 c945e8394..3778bac8d 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 'm': + case 'f': { - args->mode = atoi(optarg); + args->fmode = atoi(optarg); break; } + case 'm': + { + 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 ecbc6feb6..c78335653 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 59fdaa908..c824f689d 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 }; /****************************************************************************