diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index abeee8be2..1e0d8851d 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -201,10 +201,12 @@ endif # EXAMPLES_FOC_HAVE_TORQ config EXAMPLES_FOC_HAVE_VEL bool "FOC example velocity controller support" default EXAMPLES_FOC_SENSORLESS + select EXAMPLES_FOC_HAVE_TORQ config EXAMPLES_FOC_HAVE_POS bool "FOC example position controller support" default n + select EXAMPLES_FOC_HAVE_VEL config EXAMPLES_FOC_VELOBS bool "FOC example velocity observer support" diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index 1c7bd1965..1d2de0107 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -46,6 +46,20 @@ # endif #endif +/* Position controller needs velocity controller */ + +#if defined(CONFIG_EXAMPLES_FOC_HAVE_POS) && \ + !defined(CONFIG_EXAMPLES_FOC_HAVE_VEL) +# error Position controller needs velocity controller +#endif + +/* Velocity controller needs torque controller */ + +#if defined(CONFIG_EXAMPLES_FOC_HAVE_VEL) && \ + !defined(CONFIG_EXAMPLES_FOC_HAVE_TORQ) +# error Velocity controller needs torque controller +#endif + /* Open-loop configuration */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index 311f259ab..251f56272 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -925,29 +925,20 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor) goto no_controller; } - /* Controller */ + /* Controller. + * + * The FOC motor controller is a cascade controller: + * + * 1. Position controller sets requested velocity, + * 2. Velocity controller sets requested torque, + * 3. Torque controller sets requested motor phase voltages. + * + * NOTE: the motor torque is directly proportional to the motor + * current which is proportional to the motor set voltage + */ switch (motor->envp->cfg->mmode) { -#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ - case FOC_MMODE_TORQ: - { - /* Saturate torque */ - - f_saturate_b16(&motor->torq.des, -motor->torq_sat, - motor->torq_sat); - - /* Torque setpoint */ - - motor->torq.set = motor->torq.des; - - q_ref = motor->torq.set; - d_ref = 0; - - break; - } -#endif - #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL case FOC_MMODE_VEL: { @@ -985,14 +976,31 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor) #ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI /* PI velocit controller */ - q_ref = pi_controller_b16(&motor->vel_pi, vel_err); - d_ref = 0; + motor->torq.des = pi_controller_b16(&motor->vel_pi, + vel_err); #else # error Missing velocity controller #endif } } + /* Don't break here! pass to torque controller */ + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ + case FOC_MMODE_TORQ: + { + /* Saturate torque */ + + f_saturate_b16(&motor->torq.des, -motor->torq_sat, + motor->torq_sat); + + /* Torque setpoint */ + + motor->torq.set = motor->torq.des; + motor->torq.now = motor->foc_state.idq.q; + break; } #endif @@ -1004,6 +1012,11 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor) } } + /* Get dq ref */ + + q_ref = motor->torq.set; + d_ref = 0; + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Force open-loop current */ @@ -1547,9 +1560,7 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, ftob16(motor->envp->cfg->vel_pi_kp / 1000000.0f), ftob16(motor->envp->cfg->vel_pi_ki / 1000000.0f)); - pi_saturation_set_b16(&motor->vel_pi, - ftob16(-CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f), - ftob16(CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f)); + pi_saturation_set_b16(&motor->vel_pi, -motor->torq_sat, motor->torq_sat); pi_antiwindup_enable_b16(&motor->vel_pi, ftob16(0.99f), true); #endif diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 9a9af64df..4a9a34e11 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -908,29 +908,20 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor) goto no_controller; } - /* Controller */ + /* Controller. + * + * The FOC motor controller is a cascade controller: + * + * 1. Position controller sets requested velocity, + * 2. Velocity controller sets requested torque, + * 3. Torque controller sets requested motor phase voltages. + * + * NOTE: the motor torque is directly proportional to the motor + * current which is proportional to the motor set voltage + */ switch (motor->envp->cfg->mmode) { -#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ - case FOC_MMODE_TORQ: - { - /* Saturate torque */ - - f_saturate(&motor->torq.des, -motor->torq_sat, - motor->torq_sat); - - /* Torque setpoint */ - - motor->torq.set = motor->torq.des; - - q_ref = motor->torq.set; - d_ref = 0.0f; - - break; - } -#endif - #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL case FOC_MMODE_VEL: { @@ -968,14 +959,30 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor) #ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI /* PI velocit controller */ - q_ref = pi_controller(&motor->vel_pi, vel_err); - d_ref = 0; + motor->torq.des = pi_controller(&motor->vel_pi, + vel_err); #else # error Missing velocity controller #endif } } + /* Don't break here! pass to torque controller */ + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ + case FOC_MMODE_TORQ: + { + /* Saturate torque */ + + f_saturate(&motor->torq.des, -motor->torq_sat, + motor->torq_sat); + + /* Torque setpoint */ + + motor->torq.set = motor->torq.des; + break; } #endif @@ -987,6 +994,11 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor) } } + /* Get dq ref */ + + q_ref = motor->torq.set; + d_ref = 0.0f; + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Force open-loop current */ @@ -1536,9 +1548,7 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, (motor->envp->cfg->vel_pi_kp / 1000000.0f), (motor->envp->cfg->vel_pi_ki / 1000000.0f)); - pi_saturation_set(&motor->vel_pi, - -(CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f), - (CONFIG_EXAMPLES_FOC_VELCTRL_PI_SAT / 1000.0f)); + pi_saturation_set(&motor->vel_pi, -motor->torq_sat, motor->torq_sat); pi_antiwindup_enable(&motor->vel_pi, 0.99f, true); #endif