sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

foc/examples: initial logic to support torque, velocity and position controller modes

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 20:30:37 +01:00 committed by Xiang Xiao
parent b0eeefd0a5
commit b30f3329f6
12 changed files with 734 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
examples/foc/Kconfig
View File

@ -93,6 +93,19 @@ config EXAMPLES_FOC_HAVE_OPENLOOP
default y if EXAMPLES_FOC_SENSORLESS default y if EXAMPLES_FOC_SENSORLESS
default n 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" menu "FOC user input"
config EXAMPLES_FOC_HAVE_ADC config EXAMPLES_FOC_HAVE_ADC
@ -207,14 +220,23 @@ endmenu # FOC user input
menu "FOC controller parameters" menu "FOC controller parameters"
config EXAMPLES_FOC_OPMODE config EXAMPLES_FOC_FMODE
int "FOC operation mode" int "FOC control mode"
default 2 default 2
range 1 3 range 1 3
---help--- ---help---
1 - IDLE mode 1 - IDLE mode
2 - voltage open-loop velocity controller (default) 2 - voltage mode (default)
3 - current open-loop velocity controller 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 config EXAMPLES_FOC_OPENLOOP_Q
int "FOC open-loop Vq/Iq setting [x1000]" int "FOC open-loop Vq/Iq setting [x1000]"

11
examples/foc/foc_cfg.h
View File

@ -37,9 +37,20 @@
# error For now only open-loop supported # error For now only open-loop supported
#endif #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 */ /* Open-loop configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
# ifndef CONFIG_EXAMPLES_FOC_HAVE_VEL
# error
# endif
# ifndef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP # ifndef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
# error # error
# endif # endif

12
examples/foc/foc_fixed16_thr.c
View File

@ -80,9 +80,17 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
motor->angle_now = 0; motor->angle_now = 0;
motor->vbus = 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 */ /* Get real currents */

18
examples/foc/foc_float_thr.c
View File

@ -75,14 +75,22 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
{ {
/* Stop motor */ /* Stop motor */
motor->dq_ref.q = 0; motor->dq_ref.q = 0.0f;
motor->dq_ref.d = 0; motor->dq_ref.d = 0.0f;
motor->angle_now = 0; motor->angle_now = 0.0f;
motor->vbus = 0; motor->vbus = 0.0f;
/* Force velocity to zero */ /* 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 */ /* Get real currents */

79
examples/foc/foc_main.c
View File

@ -129,8 +129,10 @@ static void init_args(FAR struct args_s *args)
{ {
args->time = args->time =
(args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time); (args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time);
args->mode = args->fmode =
(args->mode == 0 ? CONFIG_EXAMPLES_FOC_OPMODE : args->mode); (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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
args->qparam = args->qparam =
(args->qparam == 0 ? CONFIG_EXAMPLES_FOC_OPENLOOP_Q : 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 =
(args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : 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 #ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
args->velmax = args->velmax =
(args->velmax == 0 ? (args->velmax == 0 ?
@ -150,6 +164,18 @@ static void init_args(FAR struct args_s *args)
args->velmax = args->velmax =
(args->velmax == 0 ? (args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax); 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 #endif
args->state = args->state =
@ -173,13 +199,31 @@ static int validate_args(FAR struct args_s *args)
goto errout; goto errout;
} }
/* Operation mode */ /* FOC operation mode */
if (args->mode != FOC_OPMODE_IDLE && if (args->fmode != FOC_FMODE_IDLE &&
args->mode != FOC_OPMODE_OL_V_VEL && args->fmode != FOC_FMODE_VOLTAGE &&
args->mode != FOC_OPMODE_OL_C_VEL) 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; goto errout;
} }
@ -598,12 +642,21 @@ int main(int argc, char *argv[])
/* Get configuration */ /* Get configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #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 #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)) if (args.en & (1 << i))
{ {

277
examples/foc/foc_motor_b16.c
View File

@ -113,6 +113,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor); DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Initialize velocity ramp */ /* Initialize velocity ramp */
ret = foc_ramp_init_b16(&motor->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); PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
goto errout; goto errout;
} }
#endif
/* Initialize FOC handler */ /* Initialize FOC handler */
@ -203,6 +205,32 @@ static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus)
return OK; 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 * 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); tmp1 = itob16(motor->envp->velmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1); tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
motor->vel_des = b16muli(tmp2, vel); motor->vel.des = b16muli(tmp2, vel);
return OK; 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 * Name: foc_motor_state
@ -236,21 +360,13 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
DEBUGASSERT(motor); 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 */ /* Update motor state */
switch (state) switch (state)
{ {
case FOC_EXAMPLE_STATE_FREE: case FOC_EXAMPLE_STATE_FREE:
{ {
motor->vel_set = 0; motor->dir = DIR_NONE_B16;
motor->dir = DIR_NONE_B16;
/* Force DQ vector to zero */ /* 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: 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 */ /* 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: case FOC_EXAMPLE_STATE_CW:
{ {
motor->vel_set = 0; motor->dir = DIR_CW_B16;
motor->dir = DIR_CW_B16;
break; break;
} }
case FOC_EXAMPLE_STATE_CCW: case FOC_EXAMPLE_STATE_CCW:
{ {
motor->vel_set = 0; motor->dir = DIR_CCW_B16;
motor->dir = DIR_CCW_B16;
break; 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: errout:
return ret; return ret;
} }
@ -346,23 +471,98 @@ errout:
static int foc_motor_run(FAR struct foc_motor_b16_s *motor) 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 DEBUGASSERT(motor);
* TODO: velocity observer or sensor
*/
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)
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); if (motor->envp->mmode != FOC_MMODE_VEL)
goto errout; {
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: errout:
return ret; return ret;
@ -471,7 +671,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
/* Update open-loop angle handler */ /* Update open-loop angle handler */
ain.vel = motor->vel_set; ain.vel = motor->vel.set;
ain.angle = motor->angle_now; ain.angle = motor->angle_now;
ain.dir = motor->dir; ain.dir = motor->dir;
@ -496,6 +696,19 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
ASSERT(0); 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; 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", PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->setpoint, motor->envp->id); 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) if (ret < 0)
{ {
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); PRINTF("ERROR: foc_motor_setpoint failed %d!\n", ret);
goto errout; goto errout;
} }

21
examples/foc/foc_motor_b16.h
View File

@ -46,6 +46,15 @@
* Public Type Definition * Public Type Definition
****************************************************************************/ ****************************************************************************/
/* FOC setpoint (fixed16) */
struct foc_setpoint_b16_s
{
b16_t set;
b16_t now;
b16_t des;
};
/* FOC motor data (fixed16) */ /* FOC motor data (fixed16) */
struct foc_motor_b16_s struct foc_motor_b16_s
@ -62,9 +71,15 @@ struct foc_motor_b16_s
int ctrl_state; /* Controller state */ int ctrl_state; /* Controller state */
b16_t vbus; /* Power bus voltage */ b16_t vbus; /* Power bus voltage */
b16_t angle_now; /* Phase angle now */ b16_t angle_now; /* Phase angle now */
b16_t vel_set; /* Velocity setting now */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
b16_t vel_now; /* Velocity now */ struct foc_setpoint_b16_s torq; /* Torque setpoint */
b16_t vel_des; /* Velocity destination */ #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 dir; /* Motor's direction */
b16_t per; /* Controller period in seconds */ b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */ b16_t iphase_adc; /* Iphase ADC scaling factor */

263
examples/foc/foc_motor_f32.c
View File

@ -113,6 +113,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor); DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Initialize velocity ramp */ /* Initialize velocity ramp */
ret = foc_ramp_init_f32(&motor->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); PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
goto errout; goto errout;
} }
#endif
/* Initialize FOC handler */ /* Initialize FOC handler */
@ -203,6 +205,25 @@ static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus)
return OK; 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 * 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 */ /* Update motor velocity destination */
motor->vel_des = (vel * SETPOINT_ADC_SCALE * motor->vel.des = (vel * SETPOINT_ADC_SCALE *
motor->envp->velmax / 1000.0f); motor->envp->velmax / 1000.0f);
return OK; 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 * Name: foc_motor_state
@ -229,21 +339,13 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
DEBUGASSERT(motor); 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 */ /* Update motor state */
switch (state) switch (state)
{ {
case FOC_EXAMPLE_STATE_FREE: case FOC_EXAMPLE_STATE_FREE:
{ {
motor->vel_set = 0.0f; motor->dir = DIR_NONE;
motor->dir = DIR_NONE;
/* Force DQ vector to zero */ /* 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: case FOC_EXAMPLE_STATE_STOP:
{ {
motor->vel_set = 0.0f; motor->dir = DIR_NONE;
motor->dir = DIR_NONE;
/* DQ vector not zero - active brake */ /* 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: case FOC_EXAMPLE_STATE_CW:
{ {
motor->vel_set = 0.0f; motor->dir = DIR_CW;
motor->dir = DIR_CW;
break; break;
} }
case FOC_EXAMPLE_STATE_CCW: case FOC_EXAMPLE_STATE_CCW:
{ {
motor->vel_set = 0.0f; motor->dir = DIR_CCW;
motor->dir = DIR_CCW;
break; 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: errout:
return ret; return ret;
} }
@ -346,23 +457,98 @@ errout:
static int foc_motor_run(FAR struct foc_motor_f32_s *motor) 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 DEBUGASSERT(motor);
* TODO: velocity observer or sensor
*/
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)
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); if (motor->envp->mmode != FOC_MMODE_VEL)
goto errout; {
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: errout:
return ret; return ret;
@ -471,7 +657,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
/* Update open-loop angle handler */ /* Update open-loop angle handler */
ain.vel = motor->vel_set; ain.vel = motor->vel.set;
ain.angle = motor->angle_now; ain.angle = motor->angle_now;
ain.dir = motor->dir; ain.dir = motor->dir;
@ -496,6 +682,19 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
ASSERT(0); 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; 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", PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->setpoint, motor->envp->id); 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) if (ret < 0)
{ {
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); PRINTF("ERROR: foc_motor_setpoint failed %d!\n", ret);
goto errout; goto errout;
} }

21
examples/foc/foc_motor_f32.h
View File

@ -47,6 +47,15 @@
* Public Type Definition * Public Type Definition
****************************************************************************/ ****************************************************************************/
/* FOC setpoint (float32) */
struct foc_setpoint_f32_s
{
float set;
float now;
float des;
};
/* FOC motor data (float32) */ /* FOC motor data (float32) */
struct foc_motor_f32_s struct foc_motor_f32_s
@ -63,9 +72,15 @@ struct foc_motor_f32_s
int ctrl_state; /* Controller state */ int ctrl_state; /* Controller state */
float vbus; /* Power bus voltage */ float vbus; /* Power bus voltage */
float angle_now; /* Phase angle now */ float angle_now; /* Phase angle now */
float vel_set; /* Velocity setting now */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
float vel_now; /* Velocity now */ struct foc_setpoint_f32_s torq; /* Torque setpoint */
float vel_des; /* Velocity destination */ #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 dir; /* Motor's direction */
float per; /* Controller period in seconds */ float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */ float iphase_adc; /* Iphase ADC scaling factor */

61
examples/foc/foc_parseargs.c
View File

@ -46,11 +46,22 @@ static struct option g_long_options[] =
{ {
{ "time", required_argument, 0, 't' }, { "time", required_argument, 0, 't' },
{ "help", no_argument, 0, 'h' }, { "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' }, { "vel", required_argument, 0, 'v' },
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
{ "pos", required_argument, 0, 'x' },
#endif
{ "state", required_argument, 0, 's' }, { "state", required_argument, 0, 's' },
{ "en", required_argument, 0, 'j' }, { "en", required_argument, 0, 'j' },
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
{ "oqset", required_argument, 0, 'o' }, { "oqset", required_argument, 0, 'o' },
#endif
{ "fkp", required_argument, 0, OPT_FKP }, { "fkp", required_argument, 0, OPT_FKP },
{ "fki", required_argument, 0, OPT_FKI }, { "fki", required_argument, 0, OPT_FKI },
{ 0, 0, 0, 0 } { 0, 0, 0, 0 }
@ -69,11 +80,23 @@ static void foc_help(void)
PRINTF("Usage: foc [OPTIONS]\n"); PRINTF("Usage: foc [OPTIONS]\n");
PRINTF(" [-t] run time\n"); PRINTF(" [-t] run time\n");
PRINTF(" [-h] shows this message and exits\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(" 1 - IDLE mode\n");
PRINTF(" 2 - voltage open-loop velocity \n"); PRINTF(" 2 - voltage mode \n");
PRINTF(" 3 - current open-loop velocity \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"); PRINTF(" [-v] velocity [x1000]\n");
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
PRINTF(" [-x] position [x1000]\n");
#endif
PRINTF(" [-s] motor state\n"); PRINTF(" [-s] motor state\n");
PRINTF(" 1 - motor free\n"); PRINTF(" 1 - motor free\n");
PRINTF(" 2 - motor stop\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) 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); &option_index);
if (c == -1) if (c == -1)
@ -136,17 +159,41 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
exit(0); exit(0);
} }
case 'm': case 'f':
{ {
args->mode = atoi(optarg); args->fmode = atoi(optarg);
break; 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': case 'v':
{ {
args->velmax = atoi(optarg); args->velmax = atoi(optarg);
break; break;
} }
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
case 'x':
{
args->posmax = atoi(optarg);
break;
}
#endif
case 's': case 's':
{ {

13
examples/foc/foc_parseargs.h
View File

@ -42,15 +42,24 @@
struct args_s struct args_s
{ {
int time; /* Run time limit in sec, -1 if forever */ 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
int qparam; /* Open-loop Q setting (x1000) */ int qparam; /* Open-loop Q setting (x1000) */
#endif #endif
uint32_t pi_kp; /* PI Kp (x1000) */ uint32_t pi_kp; /* PI Kp (x1000) */
uint32_t pi_ki; /* PI Ki (x1000) */ uint32_t pi_ki; /* PI Ki (x1000) */
uint32_t velmax; /* Velocity max (x1000) */
int state; /* Example state (FREE, CW, CCW, STOP) */ int state; /* Example state (FREE, CW, CCW, STOP) */
int8_t en; /* Enabled instances (bit-encoded) */ 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
}; };
/**************************************************************************** /****************************************************************************

41
examples/foc/foc_thr.h
View File

@ -48,21 +48,29 @@ enum foc_example_state_e
FOC_EXAMPLE_STATE_CCW = 4, /* CCW direction */ 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_FMODE_INVALID = 0, /* Reserved */
FOC_OPMODE_IDLE = 1, /* IDLE */ FOC_FMODE_IDLE = 1, /* IDLE */
FOC_OPMODE_OL_V_VEL = 2, /* Voltage open-loop velocity controller */ FOC_FMODE_VOLTAGE = 2, /* Voltage mode */
FOC_OPMODE_OL_C_VEL = 3, /* Current open-loop velocity controller */ FOC_FMODE_CURRENT = 3, /* Current mode */
};
/* Not supported yet */ /* Motor control mode */
#if 0 enum foc_motor_mode_e
FOC_OPMODE_CL_C_TRQ = 3, /* Current closed-loop torque controller */ {
FOC_OPMODE_CL_C_VEL = 4, /* Current closed-loop velocity controller */ FOC_MMODE_INVALID = 0, /* Reserved */
FOC_OPMODE_CL_C_POS = 5 /* Current closed-loop position controller */ #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 #endif
}; };
@ -85,13 +93,22 @@ struct foc_ctrl_env_s
int id; /* FOC device id */ int id; /* FOC device id */
int inst; /* Type specific instance counter */ int inst; /* Type specific instance counter */
int type; /* Controller type */ int type; /* Controller type */
int fmode; /* FOC control mode */
int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
int qparam; /* Open-loop Q setting (x1000) */ int qparam; /* Open-loop Q setting (x1000) */
#endif #endif
int mode; /* Operation mode */
uint32_t pi_kp; /* FOC PI Kp (x1000) */ uint32_t pi_kp; /* FOC PI Kp (x1000) */
uint32_t pi_ki; /* FOC PI Ki (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) */ uint32_t velmax; /* Velocity max (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
uint32_t posmax; /* Position max (x1000) */
#endif
}; };
/**************************************************************************** /****************************************************************************