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

examples/foc: open-loop needs initial phase alignment

Browse Source
This commit is contained in:
raiden00pl 2023-10-13 20:42:41 +02:00 committed by Xiang Xiao
parent 8e0819df04
commit fd0da7d4df
3 changed files with 115 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/foc/Kconfig
View File

@ -175,6 +175,7 @@ endif # EXAMPLES_FOC_SENSORED
config EXAMPLES_FOC_HAVE_OPENLOOP config EXAMPLES_FOC_HAVE_OPENLOOP
bool "FOC example have open-loop controller" bool "FOC example have open-loop controller"
select INDUSTRY_FOC_ANGLE_OPENLOOP select INDUSTRY_FOC_ANGLE_OPENLOOP
select EXAMPLES_FOC_HAVE_ALIGN
default EXAMPLES_FOC_SENSORLESS default EXAMPLES_FOC_SENSORLESS
config EXAMPLES_FOC_HAVE_TORQ config EXAMPLES_FOC_HAVE_TORQ

78
examples/foc/foc_motor_b16.c
View File

@ -577,6 +577,40 @@ errout:
return ret; return ret;
} }
/****************************************************************************
* Name: foc_motor_vel_reset
****************************************************************************/
static int foc_motor_vel_reset(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_b16(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_b16(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_motor_state * Name: foc_motor_state
****************************************************************************/ ****************************************************************************/
@ -637,6 +671,27 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
} }
} }
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Re-align motor if we change mode from FREE/STOP to CW/CCW otherwise,
* the open-loop may fail because the rotor position at the start is
* random.
*/
if ((motor->mq.app_state == FOC_EXAMPLE_STATE_FREE ||
motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) &&
(state == FOC_EXAMPLE_STATE_CW ||
state == FOC_EXAMPLE_STATE_CCW))
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
motor->align_done = false;
motor->angle_now = 0;
/* Reset velocity observer */
foc_motor_vel_reset(motor);
}
#endif
/* Reset current setpoint */ /* Reset current setpoint */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@ -706,29 +761,10 @@ static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor)
{ {
int ret = OK; int ret = OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_b16(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_b16(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS #ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout: ret = foc_motor_vel_reset(motor);
#endif #endif
return ret; return ret;
} }

78
examples/foc/foc_motor_f32.c
View File

@ -555,6 +555,40 @@ errout:
return ret; return ret;
} }
/****************************************************************************
* Name: foc_motor_vel_reset
****************************************************************************/
static int foc_motor_vel_reset(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_f32(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
/**************************************************************************** /****************************************************************************
* Name: foc_motor_state * Name: foc_motor_state
****************************************************************************/ ****************************************************************************/
@ -614,6 +648,27 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
} }
} }
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Re-align motor if we change mode from FREE/STOP to CW/CCW otherwise,
* the open-loop may fail because the rotor position at the start is
* random.
*/
if ((motor->mq.app_state == FOC_EXAMPLE_STATE_FREE ||
motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) &&
(state == FOC_EXAMPLE_STATE_CW ||
state == FOC_EXAMPLE_STATE_CCW))
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
motor->align_done = false;
motor->angle_now = 0.0f;
/* Reset velocity observer */
foc_motor_vel_reset(motor);
}
#endif
/* Reset current setpoint */ /* Reset current setpoint */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@ -690,29 +745,10 @@ static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
{ {
int ret = OK; int ret = OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_f32(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS #ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout: ret = foc_motor_vel_reset(motor);
#endif #endif
return ret; return ret;
} }