Что намудрили с функцией разгона Araisrobo:
Было:
Код: Выделить всё
void tcRunCycle(TP_STRUCT *tp, TC_STRUCT *tc, double *v, int *on_final_decel) {
    double discr, maxnewvel, newvel, newaccel=0;
    if(!tc->blending) tc->vel_at_blend_start = tc->currentvel;
    discr = 0.5 * tc->cycle_time * tc->currentvel - (tc->target - tc->progress);
    
    if(discr > 0.0) {
        // should never happen: means we've overshot the target
        newvel = maxnewvel = 0.0;
    } else {
        discr = 0.25 * pmSq(tc->cycle_time) - 2.0 / tc->maxaccel * discr;
        newvel = maxnewvel = -0.5 * tc->maxaccel * tc->cycle_time + tc->maxaccel * pmSqrt(discr);
    }
    
    if(newvel <= 0.0) {
        // also should never happen - if we already finished this tc, it was
        // caught above
        newvel = newaccel = 0.0;
        tc->progress = tc->target;
    } else {
        // constrain velocity
        if(newvel > tc->reqvel * tc->feed_override) newvel = tc->reqvel * tc->feed_override;
        if(newvel > tc->maxvel) newvel = tc->maxvel;
        // if the motion is not purely rotary axes (and therefore in angular units) ...
        if(!(tc->motion_type == TC_LINEAR && tc->coords.line.xyz.tmag_zero && tc->coords.line.uvw.tmag_zero)) {
            // ... clamp motion's velocity at TRAJ MAX_VELOCITY (tooltip maxvel)
            // except when it's synced to spindle position.
            if((!tc->synchronized || tc->velocity_mode) && newvel > tp->vLimit) {
                newvel = tp->vLimit;
            }
        }
        // get resulting acceleration
        newaccel = (newvel - tc->currentvel) / tc->cycle_time;
        
        // constrain acceleration and get resulting velocity
        if(newaccel > 0.0 && newaccel > tc->maxaccel) {
            newaccel = tc->maxaccel;
            newvel = tc->currentvel + newaccel * tc->cycle_time;
        }
        if(newaccel < 0.0 && newaccel < -tc->maxaccel) {
            newaccel = -tc->maxaccel;
            newvel = tc->currentvel + newaccel * tc->cycle_time;
        }
        // update position in this tc
        tc->progress += (newvel + tc->currentvel) * 0.5 * tc->cycle_time;
    }
    tc->currentvel = newvel;
    if(v) *v = newvel;
    if(on_final_decel) *on_final_decel = fabs(maxnewvel - newvel) < 0.001;
}
Т.е. простое вычисление ускорения и скорости для нормального движения исходя из текущей и максимальной скорости, ускорения, положения и заданного положения. 
Стало: 
Код: Выделить всё
/*
 Continuous form
 PT = P0 + V0T + 1/2A0T2 + 1/6JT3
 VT = V0 + A0T + 1/2 JT2
 AT = A0 + JT
 Discrete time form (let T be 1, then T^2 == 1, T^3 == 1)
 PT = PT + VT + 1/2AT + 1/6J
 VT = VT + AT + 1/2JT
 AT = AT + JT
 */
/**
 * S-curve Velocity Profiler FSM
 * Yishin Li <ysli@araisrobo.com>
 * ARAIS ROBOT TECHNOLOGY, http://www.araisrobo.com/
 **/
void tcRunCycle(TP_STRUCT *tp, TC_STRUCT *tc) 
{
    double t, t1, vel, v1, dist, req_vel;
    int immediate_state;
    double tc_target;
    
    if(tc->seamless_blend_mode == SMLBLND_ENABLE) {
        tc_target = tc->target + tc->nexttc_target;
    } else {
        tc_target = tc->target;
    }
    immediate_state = 0;
    do {
        switch (tc->accel_state) {
        case ACCEL_S0:
            // AT = AT + JT
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_accel = tc->cur_accel + tc->jerk;
            tc->cur_vel = tc->cur_vel + tc->cur_accel + 0.5 * tc->jerk;
            tc->progress = tc->progress + tc->cur_vel + 0.5 * tc->cur_accel + 1.0/6.0 * tc->jerk;
            
            // check if we hit accel limit at next BP
            if ((tc->cur_accel + tc->jerk) >= tc->maxaccel) {
                tc->cur_accel = tc->maxaccel;
                tc->accel_state = ACCEL_S1;
                break;
            }
            
            // check if we will hit velocity limit; 
            // assume we start decel from here to "accel == 0"
            //
            // AT = A0 + JT (let AT = 0 to calculate T)
            // VT = V0 + A0T + 1/2JT2
            t = ceil(tc->cur_accel / tc->jerk);
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            vel = req_vel - tc->cur_accel * t + 0.5 * tc->jerk * t * t;
            if (tc->cur_vel >= vel) {
                tc->accel_state = ACCEL_S2;
                break;
            }
            // check if we will hit progress limit
            // AT = AT + JT
            // VT = V0 + A0T + 1/2 JT^2
            // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
            // distance for S2
            t = ceil(tc->cur_accel/tc->jerk);
            dist = tc->progress + tc->cur_vel * t + 0.5 * tc->cur_accel * t * t
                   - 1.0/6.0 * tc->jerk * t * t * t;
            vel = tc->cur_vel + tc->cur_accel * t - 0.5 * tc->jerk * t * t;
            // distance for S3
            dist += (vel);
            
            /* 
            0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
            t1 = sqrt(vel/j)
            */
            t = ceil(sqrt(vel/tc->jerk));
            // AT = AT + JT
            t1 = ceil(tc->maxaccel / tc->jerk);   // max time for S4
            if (t > t1) {
                // S4 -> S5 -> S6
                dist += t1 * vel;    // dist of (S4 + S6)
                // calc decel.dist for ACCEL_S5
                // t: time for S5
                t = (vel - tc->jerk * t1 * t1) / tc->maxaccel;
                v1 = vel - 0.5 * tc->jerk * t1 * t1;
                // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
            } else {
                // S4 -> S6
                dist += t * vel;    // dist of (S4 + S6)
            }
            if (tc_target < dist) {
                tc->accel_state = ACCEL_S2;
                break;
            }
            break;
        
        case ACCEL_S1:
            // jerk is 0 at this state
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_vel = tc->cur_vel + tc->cur_accel;
            tc->progress = tc->progress + tc->cur_vel + 0.5 * tc->cur_accel;
            
            // check if we will hit velocity limit; 
            // assume we start decel from here to "accel == 0"
            //
            // AT = A0 + JT (let AT = 0 to calculate T)
            // VT = V0 + A0T + 1/2JT2
            // t = ceil(tc->cur_accel / tc->jerk);
            t = tc->cur_accel / tc->jerk;
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            vel = req_vel - tc->cur_accel * t + 0.5 * tc->jerk * t * t;
            if (tc->cur_vel >= vel) {
                tc->accel_state = ACCEL_S2;
                break;
            }
            
            // check if we will hit progress limit
            // distance for S2
            t = ceil(tc->cur_accel/tc->jerk);
            dist = tc->progress + tc->cur_vel * t + 0.5 * tc->cur_accel * t * t
                   - 1.0/6.0 * tc->jerk * t * t * t;
            vel = tc->cur_vel + tc->cur_accel * t - 0.5 * tc->jerk * t * t;
            // distance for S3
            dist += (vel);
            
            /* 
            0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
            t1 = sqrt(vel/j)
            */
            t = ceil(sqrt(vel/tc->jerk));
            // AT = AT + JT
            t1 = ceil(tc->maxaccel / tc->jerk);   // max time for S4
            if (t > t1) {
                // S4 -> S5 -> S6
                dist += t1 * vel;    // dist of (S4 + S6)
                // calc decel.dist for ACCEL_S5
                // t: time for S5
                t = (vel - tc->jerk * t1 * t1) / tc->maxaccel;
                v1 = vel - 0.5 * tc->jerk * t1 * t1;
                // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
            } else {
                // S4 -> S6
                dist += t * vel;    // dist of (S4 + S6)
            }
            if (tc_target < dist) {
                tc->accel_state = ACCEL_S2;
                break;
            }
            break;
        
        
        case ACCEL_S2: 
            // to DECELERATE to ACCEL==0
            
            // AT = AT + JT
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_accel = tc->cur_accel - tc->jerk;
            tc->cur_vel = tc->cur_vel + tc->cur_accel - 0.5 * tc->jerk;
            tc->progress = tc->progress + tc->cur_vel + 0.5 * tc->cur_accel - 1.0/6.0 * tc->jerk;
           
            // check accel == 0
            if (tc->cur_accel <= 0) {
                tc->accel_state = ACCEL_S3;
                break;
            }
            // check if we will hit velocity limit at next BP
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            // vel: velocity at next BP 
            vel = tc->cur_vel + tc->cur_accel - 1.5 * tc->jerk;
            if (vel > req_vel) {
                tc->cur_vel = vel;
                tc->accel_state = ACCEL_S3;
                break;
            } 
            
            // check if we will hit progress limit
            // refer to 2011-10-17 ysli design note
            // AT = AT + JT
            // VT = V0 + A0T + 1/2 JT^2
            // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
            vel = tc->cur_vel;
            // distance for S3
            dist = tc->progress + (vel);
            
            /* 
            0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
            t1 = sqrt(vel/j)
            */
            t = ceil(sqrt(vel/tc->jerk));
            // AT = AT + JT
            t1 = ceil(tc->maxaccel / tc->jerk);   // max time for S4
            if (t > t1) {
                // S4 -> S5 -> S6
                dist += t1 * vel;    // dist of (S4 + S6)
                // calc decel.dist for ACCEL_S5
                // t: time for S5
                t = (vel - tc->jerk * t1 * t1) / tc->maxaccel;
                v1 = vel - 0.5 * tc->jerk * t1 * t1;
                // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
            } else {
                // S4 -> S6
                dist += t * vel;    // dist of (S4 + S6)
            }
            if (tc_target < dist) {
                tc->accel_state = ACCEL_S3;
                break;
            }
            break;
        
        case ACCEL_S3:
            // PT = PT + VT + 1/2AT + 1/6JT
            // , where (jerk == 0) and (accel == 0)
            tc->cur_accel = 0;
            tc->progress = tc->progress + tc->cur_vel;
            
            // check if we will hit progress limit
            // refer to 2011-10-17 ysli design note
            // AT = AT + JT
            // VT = V0 + A0T + 1/2 JT^2
            // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
            /* 
            0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
            t1 = sqrt(vel/j)
            */
            vel = tc->cur_vel;
            // t = floor(sqrt(vel/tc->jerk) - 0.5);
            t = sqrt(vel/tc->jerk);
            // t = sqrt(vel/tc->jerk);
            // AT = AT + JT
            // t1 = floor(tc->maxaccel / tc->jerk - 0.5);   // max time for S4
            t1 = tc->maxaccel / tc->jerk;   // max time for S4
            // t1 = tc->maxaccel / tc->jerk;   // max time for S4
            if (t > t1) {
                // S4 -> S5 -> S6
                dist = tc->progress + t1 * vel;    // dist of (S4 + S6)
                // calc decel.dist for ACCEL_S5
                // t: time for S5
                // t = floor((vel - tc->maxaccel * t1) / tc->maxaccel - 0.5);
                t = (vel - tc->maxaccel * t1) / tc->maxaccel - 0.5;
                // t = (vel - tc->maxaccel * t1) / tc->maxaccel;
                v1 = vel - 0.5 * tc->jerk * t1 * t1;
                // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
            } else {
                // S4 -> S6
                dist = tc->progress + t * vel;    // dist of (S4 + S6)
            }
            // check if dist would be greater than tc_target at next cycle
            if (tc_target < (dist - vel)) {
                tc->accel_state = ACCEL_S4;
                // blending at largest velocity for G64 w/o P<tolerance>
                if (!tc->tolerance) {
                    tc->tolerance = tc->target - tc->progress; // tc->distance_to_go
                } 
                break;
            }
            // check for changes of feed_override and request-velocity
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            if ((tc->cur_vel + 1.5 * tc->jerk) < req_vel) {
                tc->accel_state = ACCEL_S0;
                break;
            } else if ((tc->cur_vel - 1.5 * tc->jerk) > req_vel) {
                tc->accel_state = ACCEL_S4;
                break;
            }
            tc->cur_vel = req_vel;
            break;
        case ACCEL_S4:
            // AT = AT + JT
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_accel = tc->cur_accel - tc->jerk;
            tc->cur_vel = tc->cur_vel + tc->cur_accel - 0.5 * tc->jerk;
            if (tc->cur_vel <= 0) {
                tc->cur_vel = 0;
                tc->accel_state = ACCEL_S3;
                break;
            }
            tc->progress = tc->progress + tc->cur_vel + 0.5 * tc->cur_accel - 1.0/6.0 * tc->jerk;
            // (accel < 0) and (jerk < 0)
            assert (tc->cur_accel < 0);
           
            // check if we hit accel limit at next BP
            if ((tc->cur_accel - tc->jerk) <= -tc->maxaccel) {
                tc->cur_accel = -tc->maxaccel;
                tc->accel_state = ACCEL_S5;
                break;
            }
            
            // should we stay in S4 and keep decel?
            // calculate dist for S6 -> S4 -> (maybe S5) -> S6
            t = - tc->cur_accel / tc->jerk;
            // target dist after switching to S6 (jerk is positive for S6)
            dist = tc->progress + tc->cur_vel * t 
                   + 0.5 * tc->cur_accel * t * t
                   + 1.0 / 6.0 * tc->jerk * t * t * t;
            // VT = V0 + A0T + 1/2JT2
            // obtain vel for S6 -> S3
            vel = tc->cur_vel + tc->cur_accel * t + 0.5 * tc->jerk * t * t;
            
            if (vel > 0) {    
                /* 
                0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
                t1 = sqrt(vel/j)
                */
                t = ceil(sqrt(vel/tc->jerk));
                // AT = AT + JT
                t1 = ceil(tc->maxaccel / tc->jerk);   // max time for S4
                if (t > t1) {
                    // S4 -> S5 -> S6
                    dist += t1 * vel;    // dist of (S4 + S6)
                    // calc decel.dist for ACCEL_S5
                    // t: time for S5
                    t = (vel - tc->jerk * t1 * t1) / tc->maxaccel;
                    v1 = vel - 0.5 * tc->jerk * t1 * t1;
                    // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                    dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
                } else {
                    // S4 -> S6
                    dist += t * vel;    // dist of (S4 + S6)
                }
            }
            // check if dist would be greater than tc_target at next cycle
            if (tc_target < (dist - (tc->cur_vel + 1.5 * tc->cur_accel - 2.1666667 * tc->jerk))) {
                tc->accel_state = ACCEL_S4;
                DPS("should stay in S4 and keep decel\n");
                break;
            }
                    
            // check if we will approaching requested velocity
            // vel should not be greater than "request velocity" after
            // starting acceleration to "accel == 0".
            //
            // AT = A0 + JT (let AT = 0 to calculate T)
            // VT = V0 + A0T + 1/2JT2
            t = - tc->cur_accel / tc->jerk;
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            if ((tc->cur_vel + tc->cur_accel * t + 0.5 * tc->jerk * t * t) <= req_vel) {
                tc->accel_state = ACCEL_S6;
                DPS("S4: hit velocity rule; move to S6\n");
                break;
            }
            
            break;
        
        case ACCEL_S5:
            // jerk is 0 at this state
            // accel < 0
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_vel = tc->cur_vel + tc->cur_accel;
            tc->progress = tc->progress + tc->cur_vel + 0.5 * tc->cur_accel;
            
            // should we stay in S5 and keep decel?
            // calculate dist for S6 -> S4 -> (maybe S5) -> S6
            t = - tc->cur_accel / tc->jerk;
            // target dist after switching to S6 (jerk is positive for S6)
            dist = tc->progress + tc->cur_vel * t 
                   + 0.5 * tc->cur_accel * t * t
                   + 1.0 / 6.0 * tc->jerk * t * t * t;
            // VT = V0 + A0T + 1/2JT2
            // obtain vel for S6 -> S3
            vel = tc->cur_vel + tc->cur_accel * t + 0.5 * tc->jerk * t * t;
            
            if (vel > 0) { 
                /* S6 -> S3 -> S4 -> S5(maybe) -> S6 */
                /* 
                0.5 * vel = vel + 0 * t1 - 0.5 * j * t1 * t1;
                t1 = sqrt(vel/j)
                */
                t = ceil(sqrt(vel/tc->jerk));
                // AT = AT + JT
                t1 = ceil(tc->maxaccel / tc->jerk);   // max time for S4
                if (t > t1) {
                    // S4 -> S5 -> S6
                    dist += t1 * vel;    // dist of (S4 + S6)
                    // calc decel.dist for ACCEL_S5
                    // t: time for S5
                    t = (vel - tc->jerk * t1 * t1) / tc->maxaccel;
                    v1 = vel - 0.5 * tc->jerk * t1 * t1;
                    // PT = P0 + V0T + 1/2A0T^2 + 1/6JT^3
                    dist += (v1 * t - 0.5 * tc->maxaccel * t * t);
                } else {
                    // S4 -> S6
                    dist += t * vel;    // dist of (S4 + S6)
                }
            }
            
            // check if dist would be greater than tc_target at next cycle
            if (tc_target < (dist - (tc->cur_vel + 1.5 * tc->cur_accel))) {
                tc->accel_state = ACCEL_S5;
                DPS("should stay in S5 and keep decel\n");
                break;
            }
            // check if we will approaching requested velocity
            // vel should not be greater than "request velocity" after
            // starting acceleration to "accel == 0".
            //
            // AT = A0 + JT (let AT = 0 to calculate T)
            // VT = V0 + A0T + 1/2JT2
            // t: cycles for accel to decel to 0
            t = - tc->cur_accel / tc->jerk;
            req_vel = tc->reqvel * tc->feed_override * tc->cycle_time;
            if (req_vel > tc->maxvel) {
                req_vel = tc->maxvel;
            }
            if ((tc->cur_vel + tc->cur_accel * t + 0.5 * tc->jerk * t * t) <= req_vel) {
                tc->accel_state = ACCEL_S6;
                break;
            }
            break;
        
        case ACCEL_S6:
            // AT = AT + JT
            // VT = VT + AT + 1/2JT
            // PT = PT + VT + 1/2AT + 1/6JT
            tc->cur_accel = tc->cur_accel + tc->jerk;
            tc->cur_vel = tc->cur_vel + tc->cur_accel + 0.5 * tc->jerk;
            if (tc->cur_vel <= 0) {
                tc->cur_accel = 0;
                tc->cur_vel = 0.5 * tc->jerk;   // give some velocity for approaching target
            }
            dist = tc->cur_vel + 0.5 * tc->cur_accel + 1.0/6.0 * tc->jerk;
            tc->progress = tc->progress + dist;
            if (tc->cur_accel >= 0) {
                tc->accel_state = ACCEL_S3;
            }
            
            break;
        
        default:
            assert(0);
        } // switch (tc->accel_state)
    } while (immediate_state);
    
    if (tc->seamless_blend_mode != SMLBLND_ENABLE) {
        if (tc->progress >= tc->target) {
            // finished
            // DPS("hit target, cur_accel(%f), cur_vel(%f)\n", tc->cur_accel, tc->cur_vel);
            tc->progress = tc->target;
            tc->cur_accel = 0;
            tc->cur_vel = 0;
        }
    }
    DPS("%11u%6d%15.5f%15.5f%15.5f%15.5f%15.5f%15.5f%15.5f\n",
        _dt, tc->accel_state, tc->reqvel * tc->feed_override * tc->cycle_time, 
        tc->cur_accel, tc->cur_vel, tc->progress/tc->target, tc->target, 
        (tc->target - tc->progress), tc_target);
    tc->distance_to_go = tc->target - tc->progress;
    //TODO: this assert will be triggered with rockman.ini: 
    //      assert (tc->cur_vel >= 0);
}
Каких-то 6 различных случая для ускорения , и вообще все как-то очень длинно и сложно. 
По идее, все, что нам надо - ввести еще один параметр для оси - a2 - максимальное изменение ускорения. Тогда функция перемещения будет 
PT = P0 + V*T + 1/2A*T^2 + 1/6A2*T^3
Т.е. имеем уравнение 3-й степени от T, которое можно решить. Зачем все эти 6 разных случаев???