From 2fb79db8b41a988c27a80fdbdd832b02b228772c Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 30 Oct 2021 13:53:32 +0200 Subject: [PATCH] examples/foc: replace the requested velocity input with a more general setpoint input. This is the inital step to support torque/velocity/position control --- examples/foc/Kconfig | 34 ++++++++++++------------- examples/foc/foc_adc.h | 30 +++++++++++----------- examples/foc/foc_fixed16_thr.c | 12 ++++----- examples/foc/foc_float_thr.c | 17 +++++++------ examples/foc/foc_main.c | 46 +++++++++++++++++++--------------- examples/foc/foc_mq.c | 4 +-- examples/foc/foc_mq.h | 4 +-- 7 files changed, 78 insertions(+), 69 deletions(-) diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 1bd955ca8..c3cf592f6 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -146,33 +146,33 @@ config EXAMPLES_FOC_VBUS_SCALE endif # EXAMPLES_FOC_VBUS_ADC choice - prompt "FOC velocity source" - default EXAMPLES_FOC_VEL_CONST + prompt "FOC setpoint source" + default EXAMPLES_FOC_SETPOINT_CONST -config EXAMPLES_FOC_VEL_CONST - bool "Use hardcoded constant velocity value" +config EXAMPLES_FOC_SETPOINT_CONST + bool "Use hardcoded constant setpiont value" -config EXAMPLES_FOC_VEL_ADC - bool "Use ADC to control velocity" +config EXAMPLES_FOC_SETPOINT_ADC + bool "Use ADC to control setpoint" select EXAMPLES_FOC_HAVE_ADC -endchoice # FOC velocity interface +endchoice # FOC setpoint interface -if EXAMPLES_FOC_VEL_CONST +if EXAMPLES_FOC_SETPOINT_CONST -config EXAMPLES_FOC_VEL_CONST_VALUE - int "FOC hardoced velocity value" - default 100000 +config EXAMPLES_FOC_SETPOINT_CONST_VALUE + int "FOC hardoced setpoint value" + default 0 -endif # EXAMPLES_FOC_VEL_CONST +endif # EXAMPLES_FOC_SETPOINT_CONST -if EXAMPLES_FOC_VEL_ADC +if EXAMPLES_FOC_SETPOINT_ADC -config EXAMPLES_FOC_VEL_ADC_MAX - int "FOC maximum velocity from ADC [x1000]" - default 100000 +config EXAMPLES_FOC_SETPOINT_ADC_MAX + int "FOC maximum setpoint from ADC [x1000]" + default 0 -endif # EXAMPLES_FOC_VEL_ADC +endif # EXAMPLES_FOC_SETPOINT_ADC config EXAMPLES_FOC_TIME_DEFAULT int "FOC run time default (sec)" diff --git a/examples/foc/foc_adc.h b/examples/foc/foc_adc.h index 38cc8222e..28fcc0434 100644 --- a/examples/foc/foc_adc.h +++ b/examples/foc/foc_adc.h @@ -40,8 +40,8 @@ /* Velocity source must be specified */ -#if defined(CONFIG_EXAMPLES_FOC_VEL_CONST) && \ - defined(CONFIG_EXAMPLES_FOC_VEL_ADC) +#if defined(CONFIG_EXAMPLES_FOC_SETPOINT_CONST) && \ + defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) # error #endif @@ -57,15 +57,15 @@ /* Velocity ADC scale factor */ -#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC -# define VEL_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX) +#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC +# define SETPOINT_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX) #endif /* If constant velocity is selected, velocity value must be provided */ -#ifdef CONFIG_EXAMPLES_FOC_VEL_CONST -# define VEL_ADC_SCALE (1) -# if CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE == 0 +#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST +# define SETPOINT_ADC_SCALE (1) +# if CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE == 0 # error # endif #endif @@ -86,10 +86,10 @@ /* AUX ADC samples support */ -# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \ +# if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) # define ADC_SAMPLES (2) -# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) || \ +# elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) || \ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) # define ADC_SAMPLES (1) # else @@ -104,14 +104,14 @@ /* Numerate ADC samples */ -# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \ +# if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) -# define VBUS_ADC_SAMPLE (0) -# define VEL_ADC_SAMPLE (1) +# define VBUS_ADC_SAMPLE (0) +# define SETPOINT_ADC_SAMPLE (1) # elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) -# define VBUS_ADC_SAMPLE (0) -# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) -# define VEL_ADC_SAMPLE (0) +# define VBUS_ADC_SAMPLE (0) +# elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) +# define SETPOINT_ADC_SAMPLE (0) # endif #endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */ diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index ccff7e9b9..6b6fc9893 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -434,7 +434,7 @@ static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel) */ tmp1 = itob16(motor->envp->velmax / 1000); - tmp2 = b16mulb16(ftob16(VEL_ADC_SCALE), tmp1); + tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1); motor->vel_des = b16muli(tmp2, vel); @@ -773,19 +773,19 @@ static int foc_motor_handle(FAR struct foc_motor_b16_s *motor, /* Update motor velocity destination */ - if (motor->mq.vel != handle->vel) + if (motor->mq.setpoint != handle->setpoint) { - PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n", - handle->vel, motor->envp->id); + PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", + handle->setpoint, motor->envp->id); - ret = foc_motor_vel(motor, handle->vel); + ret = foc_motor_vel(motor, handle->setpoint); if (ret < 0) { PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); goto errout; } - motor->mq.vel = handle->vel; + motor->mq.setpoint = handle->setpoint; } /* Update motor state */ diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index c0912118d..e1a7eed5d 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -435,7 +435,8 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel) /* Update motor velocity destination */ - motor->vel_des = (vel * VEL_ADC_SCALE * motor->envp->velmax / 1000.0f); + motor->vel_des = (vel * SETPOINT_ADC_SCALE * + motor->envp->velmax / 1000.0f); return OK; } @@ -771,21 +772,23 @@ static int foc_motor_handle(FAR struct foc_motor_f32_s *motor, motor->mq.vbus = handle->vbus; } - /* Update motor velocity destination */ + /* Update motor velocity destination + * NOTE: only velocity control supported for now + */ - if (motor->mq.vel != handle->vel) + if (motor->mq.setpoint != handle->setpoint) { - PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n", - handle->vel, motor->envp->id); + PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", + handle->setpoint, motor->envp->id); - ret = foc_motor_vel(motor, handle->vel); + ret = foc_motor_vel(motor, handle->setpoint); if (ret < 0) { PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); goto errout; } - motor->mq.vel = handle->vel; + motor->mq.setpoint = handle->setpoint; } /* Update motor state */ diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c index 66e6bbbbd..cfe72e18e 100644 --- a/examples/foc/foc_main.c +++ b/examples/foc/foc_main.c @@ -139,13 +139,19 @@ static void init_args(FAR struct args_s *args) (args->pi_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->pi_kp); args->pi_ki = (args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->pi_ki); -#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC + + /* For now only velocity control supported */ + +#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC args->velmax = - (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_ADC_MAX : args->velmax); + (args->velmax == 0 ? + CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->velmax); #else args->velmax = - (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE : args->velmax); + (args->velmax == 0 ? + CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax); #endif + args->state = (args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state); args->en = (args->en == -1 ? INST_EN_DEAFULT : args->en); @@ -248,12 +254,12 @@ static int foc_vbus_send(mqd_t mqd, uint32_t vbus) } /**************************************************************************** - * Name: foc_vel_send + * Name: foc_setpoint_send ****************************************************************************/ -static int foc_vel_send(mqd_t mqd, uint32_t vel) +static int foc_setpoint_send(mqd_t mqd, uint32_t setpoint) { - return foc_mq_send(mqd, CONTROL_MQ_MSG_VEL, (FAR void *)&vel); + return foc_mq_send(mqd, CONTROL_MQ_MSG_SETPOINT, (FAR void *)&setpoint); } /**************************************************************************** @@ -513,10 +519,10 @@ int main(int argc, char *argv[]) #endif uint32_t state = 0; uint32_t vbus_raw = 0; - int32_t vel_raw = 0; + int32_t sp_raw = 0; bool vbus_update = false; bool state_update = false; - bool vel_update = false; + bool sp_update = false; bool terminate = false; bool started = false; int ret = OK; @@ -639,9 +645,9 @@ int main(int argc, char *argv[]) vbus_update = true; vbus_raw = VBUS_CONST_VALUE; #endif -#ifndef CONFIG_EXAMPLES_FOC_VEL_ADC - vel_update = true; - vel_raw = 1; +#ifndef CONFIG_EXAMPLES_FOC_SETPOINT_ADC + sp_update = true; + sp_raw = 1; #endif state_update = true; @@ -756,12 +762,12 @@ int main(int argc, char *argv[]) vbus_update = true; # endif -# ifdef CONFIG_EXAMPLES_FOC_VEL_ADC +# ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC /* Get raw VEL */ - vel_raw = adc_sample[VEL_ADC_SAMPLE].am_data; + sp_raw = adc_sample[SETPOINT_ADC_SAMPLE].am_data; - vel_update = true; + sp_update = true; # endif /* ADC trigger next cycle */ @@ -825,20 +831,20 @@ int main(int argc, char *argv[]) /* 3. Update motor velocity */ - if (vel_update == true) + if (sp_update == true) { for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) { if (args.en & (1 << i)) { - PRINTFV("Send velocity to %d\n", i); + PRINTFV("Send setpoint = %" PRIu32 "to %d\n", sp_raw, i); - /* Send VELOCITY to threads */ + /* Send setpoint to threads */ - ret = foc_vel_send(mqd[i], vel_raw); + ret = foc_setpoint_send(mqd[i], sp_raw); if (ret < 0) { - PRINTF("ERROR: foc_vel_send failed %d\n", ret); + PRINTF("ERROR: foc_setpoint_send failed %d\n", ret); goto errout; } } @@ -846,7 +852,7 @@ int main(int argc, char *argv[]) /* Reset flag */ - vel_update = false; + sp_update = false; } /* 4. One time start */ diff --git a/examples/foc/foc_mq.c b/examples/foc/foc_mq.c index a28116f3d..e33617034 100644 --- a/examples/foc/foc_mq.c +++ b/examples/foc/foc_mq.c @@ -87,9 +87,9 @@ int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h) break; } - case CONTROL_MQ_MSG_VEL: + case CONTROL_MQ_MSG_SETPOINT: { - memcpy(&h->vel, &buffer[1], 4); + memcpy(&h->setpoint, &buffer[1], 4); break; } diff --git a/examples/foc/foc_mq.h b/examples/foc/foc_mq.h index 319cc94f3..4c94c93d7 100644 --- a/examples/foc/foc_mq.h +++ b/examples/foc/foc_mq.h @@ -50,7 +50,7 @@ enum foc_thr_msg_e CONTROL_MQ_MSG_INVALID = 0, CONTROL_MQ_MSG_VBUS = 1, CONTROL_MQ_MSG_APPSTATE = 2, - CONTROL_MQ_MSG_VEL = 3, + CONTROL_MQ_MSG_SETPOINT = 3, CONTROL_MQ_MSG_START = 4, CONTROL_MQ_MSG_KILL = 5 }; @@ -63,7 +63,7 @@ struct foc_mq_s bool start; int app_state; uint32_t vbus; - uint32_t vel; + uint32_t setpoint; }; /****************************************************************************