From 4ef6f2f6f994ecc9ef8cb5c9c317fec58d793036 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Fri, 25 Jul 2025 13:42:14 +0300 Subject: [PATCH] fixed bug in moving model --- moving_model/Tramp.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/moving_model/Tramp.c b/moving_model/Tramp.c index ea26f88..bd5fef3 100644 --- a/moving_model/Tramp.c +++ b/moving_model/Tramp.c @@ -122,7 +122,10 @@ static int calc(moveparam_t *x, double t){ DBG("dx01=%g, dt01=%g", dx01, dt01); }else{ // increase or decrease speed without stopping phase dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; - double a = (curspeed > setspeed) ? -Max.accel : Max.accel; + double a = sign * Max.accel; + if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;} + else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;} + //double a = (curspeed > setspeed) ? -Max.accel : Max.accel; dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); if(dx01 + dx23 > Dx){ // calculate max speed @@ -150,7 +153,7 @@ static int calc(moveparam_t *x, double t){ dx23 = setspeed * dt23 / 2.; // calculate dx12 and dt12 double dx12 = Dx - dx01 - dx23; - if(dx12 < 0.){ + if(dx12 < -coord_tolerance){ DBG("Oops, WTF?"); return FALSE; }