diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index 6b6fc9893..b41f31344 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -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; } diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index e1a7eed5d..3a5ef25b0 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -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; }