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

industry/foc/ramp: fix acceleration in CCW direction and simplify code

Browse Source
This commit is contained in:
raiden00pl 2023-10-17 13:10:02 +02:00 committed by Xiang Xiao
parent 4652c5516d
commit f117860a16
2 changed files with 72 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
industry/foc/fixed16/foc_ramp.c
View File

@ -85,6 +85,8 @@ int foc_ramp_init_b16(FAR struct foc_ramp_b16_s *ramp, b16_t per,
int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
b16_t now, FAR b16_t *set)
{
b16_t sign = b16ONE;
DEBUGASSERT(ramp);
/* Check if we require soft start/stop operation.
@ -95,14 +97,42 @@ int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
{
ramp->diff = des - *set;
if (ramp->diff >= ramp->ramp_thr)
/* If we change direction then the first vel target is 0 */
if (des * (*set) < 0)
{
ramp->diff = -now;
des = 0;
}
if (now >= 0 && ramp->diff >= ramp->ramp_thr)
{
/* Soft start in CW direction */
sign = b16ONE;
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else if (ramp->diff <= -ramp->ramp_thr)
else if (now >= 0 && ramp->diff <= -ramp->ramp_thr)
{
/* Soft stop in CW direction */
sign = -b16ONE;
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else if (now < 0 && ramp->diff >= ramp->ramp_thr)
{
/* Soft stop in CCW direction */
sign = b16ONE;
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else if (now < 0 && ramp->diff <= -ramp->ramp_thr)
{
/* Soft start in CCW direction */
sign = -b16ONE;
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else
{
/* Just set new setpoint */
@ -129,38 +159,18 @@ int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
case RAMP_MODE_SOFTSTART:
{
if (des - *set >= ramp->ramp_thr)
{
/* Increase setpoint with ramp */
/* Increase setpoint with ramp */
*set = now + ramp->ramp_acc_per;
}
else
{
/* Set final setpoint and exit soft start */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
*set = now + b16mulb16(sign, ramp->ramp_acc_per);
break;
}
case RAMP_MODE_SOFTSTOP:
{
if (des - *set <= -ramp->ramp_thr)
{
/* Stop motor with ramp */
/* Stop motor with ramp */
*set = now - ramp->ramp_dec_per;
}
else
{
/* Set final setpoint and exit soft stop */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
*set = now + b16mulb16(sign, ramp->ramp_dec_per);
break;
}

62
industry/foc/float/foc_ramp.c
View File

@ -85,6 +85,8 @@ int foc_ramp_init_f32(FAR struct foc_ramp_f32_s *ramp, float per,
int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
float now, FAR float *set)
{
float sign = 1.0f;
DEBUGASSERT(ramp);
/* Check if we require soft start/stop operation.
@ -95,14 +97,42 @@ int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
{
ramp->diff = des - *set;
if (ramp->diff >= ramp->ramp_thr)
/* If we change direction then the first vel target is 0 */
if (des * (*set) < 0.0f)
{
ramp->diff = -now;
des = 0.0f;
}
if (now >= 0.0f && ramp->diff >= ramp->ramp_thr)
{
/* Soft start in CW direction */
sign = 1.0f;
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else if (ramp->diff <= -ramp->ramp_thr)
else if (now >= 0.0f && ramp->diff <= -ramp->ramp_thr)
{
/* Soft stop in CW direction */
sign = -1.0f;
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else if (now < 0.0f && ramp->diff >= ramp->ramp_thr)
{
/* Soft stop in CCW direction */
sign = 1.0f;
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else if (now < 0.0f && ramp->diff <= -ramp->ramp_thr)
{
/* Soft start in CCW direction */
sign = -1.0f;
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else
{
/* Just set new setpoint */
@ -129,38 +159,18 @@ int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
case RAMP_MODE_SOFTSTART:
{
if (des - *set >= ramp->ramp_thr)
{
/* Increase setpoin with ramp */
/* Increase setpoin with ramp */
*set = now + ramp->ramp_acc_per;
}
else
{
/* Set final setpoint and exit soft start */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
*set = now + sign * ramp->ramp_acc_per;
break;
}
case RAMP_MODE_SOFTSTOP:
{
if (des - *set <= -ramp->ramp_thr)
{
/* Stop motor with ramp */
/* Stop motor with ramp */
*set = now - ramp->ramp_dec_per;
}
else
{
/* Set final setpint and exit soft stop */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
*set = now + sign * ramp->ramp_dec_per;
break;
}