только что-то сейчас не догоняю по какой формуле они скорость считают - надо спать
Код: Выделить всё
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;
// tc - содержит данные о предыдущем шаге, прогрессе, задании и максимальных скоростях / ускорениях
// считаем дискриминант. точнее пока не его, смотрим, не проедем ли мы за необходимую точку за этот период
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 {
// а вот теперь дискриминант от d(t) = v0*t + a*t*t/2
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);
}1
if(newvel <= 0.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;
}
)