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

examples/foc: replace the requested velocity input with a more general setpoint input.

Browse Source 
This is the inital step to support torque/velocity/position control
This commit is contained in:
raiden00pl 2021-10-30 13:53:32 +02:00 committed by Xiang Xiao
parent 4001d27571
commit 2fb79db8b4
7 changed files with 78 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
examples/foc/Kconfig
View File

@ -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)"

26
examples/foc/foc_adc.h
View File

@ -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 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)
# elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC)
# define SETPOINT_ADC_SAMPLE (0)
# endif
#endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */

12
examples/foc/foc_fixed16_thr.c
View File

@ -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 */

17
examples/foc/foc_float_thr.c
View File

@ -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 */

46
examples/foc/foc_main.c
View File

@ -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 */

4
examples/foc/foc_mq.c
View File

@ -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;
}

4
examples/foc/foc_mq.h
View File

@ -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;
};
/****************************************************************************