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

examples/foc: separate motor control logic from motor state logic

Browse Source
This commit is contained in:
raiden00pl 2021-10-30 14:45:03 +02:00 committed by Xiang Xiao
parent 3ba18e5407
commit 2d034ed09a
2 changed files with 86 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
examples/foc/foc_fixed16_thr.c
View File

@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
* Name: foc_motor_run
* Name: foc_motor_get
****************************************************************************/
static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
static int foc_motor_get(FAR struct foc_motor_b16_s *motor)
{
struct foc_angle_in_b16_s ain;
struct foc_angle_out_b16_s aout;
@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
ASSERT(0);
}
return ret;
}
/****************************************************************************
* Name: foc_motor_control
****************************************************************************/
static int foc_motor_control(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
@ -933,12 +946,21 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
}
}
/* Run motor controller */
/* Get motor state */
ret = foc_motor_run(&motor);
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
/* Motor control */
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}

64
examples/foc/foc_float_thr.c
View File

@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
* Name: foc_motor_run
* Name: foc_motor_get
****************************************************************************/
static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
static int foc_motor_get(FAR struct foc_motor_f32_s *motor)
{
struct foc_angle_in_f32_s ain;
struct foc_angle_out_f32_s aout;
@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
ASSERT(0);
}
return ret;
}
/****************************************************************************
* Name: foc_motor_control
****************************************************************************/
static int foc_motor_control(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* No velocity feedback - assume that velocity now is velocity set
* TODO: velocity observer or sensor
*/
motor->vel_now = motor->vel_set;
/* Run velocity ramp controller */
ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
motor->vel_now, &motor->vel_set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
@ -935,12 +948,21 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
}
}
/* Run motor controller */
/* Get motor state */
ret = foc_motor_run(&motor);
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
/* Motor control */
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}