This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
The following commit(s) were added to refs/heads/master by this push:
new f117860a1 industry/foc/ramp: fix acceleration in CCW direction and
simplify code
f117860a1 is described below
commit f117860a16da9966e35e0c0459facf31ac4e0dc4
Author: raiden00pl <raide...@railab.me>
AuthorDate: Tue Oct 17 13:10:02 2023 +0200
industry/foc/ramp: fix acceleration in CCW direction and simplify code
---
industry/foc/fixed16/foc_ramp.c | 64 ++++++++++++++++++++++++-----------------
industry/foc/float/foc_ramp.c | 64 ++++++++++++++++++++++++-----------------
2 files changed, 74 insertions(+), 54 deletions(-)
diff --git a/industry/foc/fixed16/foc_ramp.c b/industry/foc/fixed16/foc_ramp.c
index 237ebd3b8..443f021fe 100644
--- a/industry/foc/fixed16/foc_ramp.c
+++ b/industry/foc/fixed16/foc_ramp.c
@@ -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 */
-
- *set = now - ramp->ramp_dec_per;
- }
- else
- {
- /* Set final setpoint and exit soft stop */
-
- *set = des;
- ramp->ramp_mode = RAMP_MODE_NORMAL;
- }
+ /* Stop motor with ramp */
+
+ *set = now + b16mulb16(sign, ramp->ramp_dec_per);
break;
}
diff --git a/industry/foc/float/foc_ramp.c b/industry/foc/float/foc_ramp.c
index 566ee1144..ead702752 100644
--- a/industry/foc/float/foc_ramp.c
+++ b/industry/foc/float/foc_ramp.c
@@ -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 */
-
- *set = now - ramp->ramp_dec_per;
- }
- else
- {
- /* Set final setpint and exit soft stop */
-
- *set = des;
- ramp->ramp_mode = RAMP_MODE_NORMAL;
- }
+ /* Stop motor with ramp */
+
+ *set = now + sign * ramp->ramp_dec_per;
break;
}
