EmcPose tcGetPos(TC_STRUCT *tc)
{
EmcPose v;
PmPose v1;
PmPose v2;
if (0 == tc)
{
v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
v1.rot.s = 1.0;
v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
return v;
}
/* note: was if tc->targetPos <= 0.0 return basePos */
if (tc->type == TC_LINEAR)
{
pmLinePoint(&tc->line, tc->currentPos, &v1);
}
else if (tc->type == TC_CIRCULAR)
{
pmCirclePoint(&tc->circle, tc->currentPos / tc->circle.radius, &v1);
}
else
{
v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
v1.rot.s = 1.0;
v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
}
v.tran = v1.tran;
if(tc->abc_mag > 1e-6)
{
if(tc->tmag > 1e-6 )
{
pmLinePoint(&tc->line_abc,(tc->currentPos *tc->abc_mag /tc->tmag),&v2);
v.a = v2.tran.x;
v.b = v2.tran.y;
v.c = v2.tran.z;
}
else
{
pmLinePoint(&tc->line_abc,tc->currentPos,&v2);
v.a = v2.tran.x;
v.b = v2.tran.y;
v.c = v2.tran.z;
}
}
else
{
v.a = v.b = v.c = 0.0;
}
return v;
}
int tpRunCycle(TP_STRUCT * tp, long period)
{
// vel = (new position - old position) / cycle time
// (two position points required)
//
// acc = (new vel - old vel) / cycle time
// (three position points required)
TC_STRUCT *tc, *nexttc;
double primary_vel;
int on_final_decel;
EmcPose primary_before, primary_after;
EmcPose secondary_before, secondary_after;
EmcPose primary_displacement, secondary_displacement;
static double spindleoffset;
static int waiting_for_index = MOTION_INVALID_ID;
static int waiting_for_atspeed = MOTION_INVALID_ID;
double save_vel;
static double revs;
EmcPose target;
emcmotStatus->tcqlen = tcqLen(&tp->queue);
emcmotStatus->requested_vel = 0.0;
tc = tcqItem(&tp->queue, 0, period);
if(!tc) {
// this means the motion queue is empty. This can represent
// the end of the program OR QUEUE STARVATION. In either case,
// I want to stop. Some may not agree that's what it should do.
tcqInit(&tp->queue);
tp->goalPos = tp->currentPos;
tp->done = 1;
tp->depth = tp->activeDepth = 0;
tp->aborting = 0;
tp->execId = 0;
tp->motionType = 0;
tpResume(tp);
// when not executing a move, use the current enable flags
emcmotStatus->enables_queued = emcmotStatus->enables_new;
return 0;
}
if (tc->target == tc->progress && waiting_for_atspeed != tc->id) {
// if we're synced, and this move is ending, save the
// spindle position so the next synced move can be in
// the right place.
if(tc->synchronized)
spindleoffset += tc->target/tc->uu_per_rev;
else
spindleoffset = 0.0;
if(tc->indexrotary != -1) {
// this was an indexing move, so before we remove it we must
// relock the axis
tpSetRotaryUnlock(tc->indexrotary, 0);
// if it is now locked, fall through and remove the finished move.
// otherwise, just come back later and check again
if(tpGetRotaryIsUnlocked(tc->indexrotary))
return 0;
}
// done with this move
tcqRemove(&tp->queue, 1);
// so get next move
tc = tcqItem(&tp->queue, 0, period);
if(!tc) return 0;
}
// now we have the active tc. get the upcoming one, if there is one.
// it's not an error if there isn't another one - we just don't
// do blending. This happens in MDI for instance.
if(!emcmotDebug->stepping && tc->blend_with_next)
nexttc = tcqItem(&tp->queue, 1, period);
else
nexttc = NULL;
{
int this_synch_pos = tc->synchronized && !tc->velocity_mode;
int next_synch_pos = nexttc && nexttc->synchronized && !nexttc->velocity_mode;
if(!this_synch_pos && next_synch_pos) {
// we'll have to wait for spindle sync; might as well
// stop at the right place (don't blend)
tc->blend_with_next = 0;
nexttc = NULL;
}
}
if(nexttc && nexttc->atspeed) {
// we'll have to wait for the spindle to be at-speed; might as well
// stop at the right place (don't blend), like above
tc->blend_with_next = 0;
nexttc = NULL;
}
if(tp->aborting) {
// an abort message has come
if( MOTION_ID_VALID(waiting_for_index) ||
MOTION_ID_VALID(waiting_for_atspeed) ||
(tc->currentvel == 0.0 && !nexttc) ||
(tc->currentvel == 0.0 && nexttc && nexttc->currentvel == 0.0) ) {
tcqInit(&tp->queue);
tp->goalPos = tp->currentPos;
tp->done = 1;
tp->depth = tp->activeDepth = 0;
tp->aborting = 0;
tp->execId = 0;
tp->motionType = 0;
tp->synchronized = 0;
waiting_for_index = MOTION_INVALID_ID;
waiting_for_atspeed = MOTION_INVALID_ID;
emcmotStatus->spindleSync = 0;
tpResume(tp);
return 0;
} else {
tc->reqvel = 0.0;
if(nexttc) nexttc->reqvel = 0.0;
}
}
// this is no longer the segment we were waiting_for_index for
if (MOTION_ID_VALID(waiting_for_index) && waiting_for_index != tc->id)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"Was waiting for index on motion id %d, but reached id %d\n",
waiting_for_index, tc->id);
waiting_for_index = MOTION_INVALID_ID;
}
if (MOTION_ID_VALID(waiting_for_atspeed) && waiting_for_atspeed != tc->id)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"Was waiting for atspeed on motion id %d, but reached id %d\n",
waiting_for_atspeed, tc->id);
waiting_for_atspeed = MOTION_INVALID_ID;
}
// check for at-speed before marking the tc active
if (MOTION_ID_VALID(waiting_for_atspeed)) {
if(!emcmotStatus->spindle_is_atspeed) {
/* spindle is still not at the right speed: wait */
return 0;
} else {
waiting_for_atspeed = MOTION_INVALID_ID;
}
}
if(tc->active == 0) {
// this means this tc is being read for the first time.
// wait for atspeed, if motion requested it. also, force
// atspeed check for the start of all spindle synchronized
// moves.
if((tc->atspeed || (tc->synchronized && !tc->velocity_mode && !emcmotStatus->spindleSync)) &&
!emcmotStatus->spindle_is_atspeed) {
waiting_for_atspeed = tc->id;
return 0;
}
if (tc->indexrotary != -1) {
// request that the axis unlock
tpSetRotaryUnlock(tc->indexrotary, 1);
// if it is unlocked, fall through and start the move.
// otherwise, just come back later and check again
if (!tpGetRotaryIsUnlocked(tc->indexrotary))
return 0;
}
tc->active = 1;
tc->currentvel = 0;
tp->depth = tp->activeDepth = 1;
tp->motionType = tc->canon_motion_type;
tc->blending = 0;
// honor accel constraint in case we happen to make an acute angle
// with the next segment.
if(tc->blend_with_next)
tc->maxaccel /= 2.0;
if(tc->synchronized) {
if(!tc->velocity_mode && !emcmotStatus->spindleSync) {
// if we aren't already synced, wait
waiting_for_index = tc->id;
// ask for an index reset
emcmotStatus->spindle_index_enable = 1;
spindleoffset = 0.0;
// don't move: wait
return 0;
}
}
}
if (MOTION_ID_VALID(waiting_for_index)) {
if(emcmotStatus->spindle_index_enable) {
/* haven't passed index yet */
return 0;
} else {
/* passed index, start the move */
emcmotStatus->spindleSync = 1;
waiting_for_index = MOTION_INVALID_ID;
tc->sync_accel=1;
revs=0;
}
}
if (tc->motion_type == TC_RIGIDTAP) {
static double old_spindlepos;
double new_spindlepos = emcmotStatus->spindleRevs;
if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos;
switch (tc->coords.rigidtap.state) {
case TAPPING:
if (tc->progress >= tc->coords.rigidtap.reversal_target) {
// command reversal
emcmotStatus->spindle.speed *= -1;
tc->coords.rigidtap.state = REVERSING;
}
break;
case REVERSING:
if (new_spindlepos < old_spindlepos) {
PmPose start, end;
PmLine *aux = &tc->coords.rigidtap.aux_xyz;
// we've stopped, so set a new target at the original position
tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + spindleoffset;
pmLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start);
end = tc->coords.rigidtap.xyz.start;
pmLineInit(aux, start, end);
tc->coords.rigidtap.reversal_target = aux->tmag;
tc->target = aux->tmag + 10. * tc->uu_per_rev;
tc->progress = 0.0;
tc->coords.rigidtap.state = RETRACTION;
}
old_spindlepos = new_spindlepos;
break;
case RETRACTION:
if (tc->progress >= tc->coords.rigidtap.reversal_target) {
emcmotStatus->spindle.speed *= -1;
tc->coords.rigidtap.state = FINAL_REVERSAL;
}
break;
case FINAL_REVERSAL:
if (new_spindlepos > old_spindlepos) {
PmPose start, end;
PmLine *aux = &tc->coords.rigidtap.aux_xyz;
pmLinePoint(aux, tc->progress, &start);
end = tc->coords.rigidtap.xyz.start;
pmLineInit(aux, start, end);
tc->target = aux->tmag;
tc->progress = 0.0;
tc->synchronized = 0;
tc->reqvel = tc->maxvel;
tc->coords.rigidtap.state = FINAL_PLACEMENT;
}
old_spindlepos = new_spindlepos;
break;
case FINAL_PLACEMENT:
// this is a regular move now, it'll stop at target above.
break;
}
}
if(!tc->synchronized) emcmotStatus->spindleSync = 0;
if(nexttc && nexttc->active == 0) {
// this means this tc is being read for the first time.
nexttc->currentvel = 0;
tp->depth = tp->activeDepth = 1;
nexttc->active = 1;
nexttc->blending = 0;
// honor accel constraint if we happen to make an acute angle with the
// above segment or the following one
if(tc->blend_with_next || nexttc->blend_with_next)
nexttc->maxaccel /= 2.0;
}
if(tc->synchronized) {
double pos_error;
double oldrevs = revs;
if(tc->velocity_mode) {
pos_error = fabs(emcmotStatus->spindleSpeedIn) * tc->uu_per_rev;
if(nexttc) pos_error -= nexttc->progress; /* ?? */
if(!tp->aborting) {
tc->feed_override = emcmotStatus->net_feed_scale;
tc->reqvel = pos_error;
}
} else {
double spindle_vel, target_vel;
double new_spindlepos = emcmotStatus->spindleRevs;
if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos;
if(tc->motion_type == TC_RIGIDTAP &&
(tc->coords.rigidtap.state == RETRACTION ||
tc->coords.rigidtap.state == FINAL_REVERSAL))
revs = tc->coords.rigidtap.spindlerevs_at_reversal -
new_spindlepos;
else
revs = new_spindlepos;
pos_error = (revs - spindleoffset) * tc->uu_per_rev - tc->progress;
if(nexttc) pos_error -= nexttc->progress;
if(tc->sync_accel) {
// detect when velocities match, and move the target accordingly.
// acceleration will abruptly stop and we will be on our new target.
spindle_vel = revs/(tc->cycle_time * tc->sync_accel++);
target_vel = spindle_vel * tc->uu_per_rev;
if(tc->currentvel >= target_vel) {
// move target so as to drive pos_error to 0 next cycle
spindleoffset = revs - tc->progress/tc->uu_per_rev;
tc->sync_accel = 0;
tc->reqvel = target_vel;
} else {
// beginning of move and we are behind: accel as fast as we can
tc->reqvel = tc->maxvel;
}
} else {
// we have synced the beginning of the move as best we can -
// track position (minimize pos_error).
double errorvel;
spindle_vel = (revs - oldrevs) / tc->cycle_time;
target_vel = spindle_vel * tc->uu_per_rev;
errorvel = pmSqrt(fabs(pos_error) * tc->maxaccel);
if(pos_error<0) errorvel = -errorvel;
tc->reqvel = target_vel + errorvel;
}
tc->feed_override = 1.0;
}
if(tc->reqvel < 0.0) tc->reqvel = 0.0;
if(nexttc) {
if (nexttc->synchronized) {
nexttc->reqvel = tc->reqvel;
nexttc->feed_override = 1.0;
if(nexttc->reqvel < 0.0) nexttc->reqvel = 0.0;
} else {
nexttc->feed_override = emcmotStatus->net_feed_scale;
}
}
} else {
tc->feed_override = emcmotStatus->net_feed_scale;
if(nexttc) {
nexttc->feed_override = emcmotStatus->net_feed_scale;
}
}
/* handle pausing */
if(tp->pausing && (!tc->synchronized || tc->velocity_mode)) {
tc->feed_override = 0.0;
if(nexttc) {
nexttc->feed_override = 0.0;
}
}
// calculate the approximate peak velocity the nexttc will hit.
// we know to start blending it in when the current tc goes below
// this velocity...
if(nexttc && nexttc->maxaccel) {
tc->blend_vel = nexttc->maxaccel *
pmSqrt(nexttc->target / nexttc->maxaccel);
if(tc->blend_vel > nexttc->reqvel * nexttc->feed_override) {
// segment has a cruise phase so let's blend over the
// whole accel period if possible
tc->blend_vel = nexttc->reqvel * nexttc->feed_override;
}
if(tc->maxaccel < nexttc->maxaccel)
tc->blend_vel *= tc->maxaccel/nexttc->maxaccel;
if(tc->tolerance) {
/* see diagram blend.fig. T (blend tolerance) is given, theta
* is calculated from dot(s1,s2)
*
* blend criteria: we are decelerating at the end of segment s1
* and we pass distance d from the end.
* find the corresponding velocity v when passing d.
*
* in the drawing note d = 2T/cos(theta)
*
* when v1 is decelerating at a to stop, v = at, t = v/a
* so required d = .5 a (v/a)^2
*
* equate the two expressions for d and solve for v
*/
double tblend_vel;
double dot;
double theta;
PmCartesian v1, v2;
v1 = tcGetEndingUnitVector(tc);
v2 = tcGetStartingUnitVector(nexttc);
pmCartCartDot(v1, v2, &dot);
theta = acos(-dot)/2.0;
if(cos(theta) > 0.001) {
tblend_vel = 2.0 * pmSqrt(tc->maxaccel * tc->tolerance / cos(theta));
if(tblend_vel < tc->blend_vel)
tc->blend_vel = tblend_vel;
}
}
}
primary_before = tcGetPos(tc);
tcRunCycle(tp, tc, &primary_vel, &on_final_decel);
primary_after = tcGetPos(tc);
pmCartCartSub(primary_after.tran, primary_before.tran,
&primary_displacement.tran);
primary_displacement.a = primary_after.a - primary_before.a;
primary_displacement.b = primary_after.b - primary_before.b;
primary_displacement.c = primary_after.c - primary_before.c;
primary_displacement.u = primary_after.u - primary_before.u;
primary_displacement.v = primary_after.v - primary_before.v;
primary_displacement.w = primary_after.w - primary_before.w;
// blend criteria
if((tc->blending && nexttc) ||
(nexttc && on_final_decel && primary_vel < tc->blend_vel)) {
// make sure we continue to blend this segment even when its
// accel reaches 0 (at the very end)
tc->blending = 1;
// hack to show blends in axis
// tp->motionType = 0;
if(tc->currentvel > nexttc->currentvel) {
target = tcGetEndpoint(tc);
tp->motionType = tc->canon_motion_type;
emcmotStatus->distance_to_go = tc->target - tc->progress;
emcmotStatus->enables_queued = tc->enables;
// report our line number to the guis
tp->execId = tc->id;
emcmotStatus->requested_vel = tc->reqvel;
} else {
tpToggleDIOs(nexttc); //check and do DIO changes
target = tcGetEndpoint(nexttc);
tp->motionType = nexttc->canon_motion_type;
emcmotStatus->distance_to_go = nexttc->target - nexttc->progress;
emcmotStatus->enables_queued = nexttc->enables;
// report our line number to the guis
tp->execId = nexttc->id;
emcmotStatus->requested_vel = nexttc->reqvel;
}
emcmotStatus->current_vel = tc->currentvel + nexttc->currentvel;
secondary_before = tcGetPos(nexttc);
save_vel = nexttc->reqvel;
nexttc->reqvel = nexttc->feed_override > 0.0 ?
((tc->vel_at_blend_start - primary_vel) / nexttc->feed_override) :
0.0;
tcRunCycle(tp, nexttc, NULL, NULL);
nexttc->reqvel = save_vel;
secondary_after = tcGetPos(nexttc);
pmCartCartSub(secondary_after.tran, secondary_before.tran,
&secondary_displacement.tran);
secondary_displacement.a = secondary_after.a - secondary_before.a;
secondary_displacement.b = secondary_after.b - secondary_before.b;
secondary_displacement.c = secondary_after.c - secondary_before.c;
secondary_displacement.u = secondary_after.u - secondary_before.u;
secondary_displacement.v = secondary_after.v - secondary_before.v;
secondary_displacement.w = secondary_after.w - secondary_before.w;
pmCartCartAdd(tp->currentPos.tran, primary_displacement.tran,
&tp->currentPos.tran);
pmCartCartAdd(tp->currentPos.tran, secondary_displacement.tran,
&tp->currentPos.tran);
tp->currentPos.a += primary_displacement.a + secondary_displacement.a;
tp->currentPos.b += primary_displacement.b + secondary_displacement.b;
tp->currentPos.c += primary_displacement.c + secondary_displacement.c;
tp->currentPos.u += primary_displacement.u + secondary_displacement.u;
tp->currentPos.v += primary_displacement.v + secondary_displacement.v;
tp->currentPos.w += primary_displacement.w + secondary_displacement.w;
} else {
tpToggleDIOs(tc); //check and do DIO changes
target = tcGetEndpoint(tc);
tp->motionType = tc->canon_motion_type;
emcmotStatus->distance_to_go = tc->target - tc->progress;
tp->currentPos = primary_after;
emcmotStatus->current_vel = tc->currentvel;
emcmotStatus->requested_vel = tc->reqvel;
emcmotStatus->enables_queued = tc->enables;
// report our line number to the guis
tp->execId = tc->id;
}
emcmotStatus->dtg.tran.x = target.tran.x - tp->currentPos.tran.x;
emcmotStatus->dtg.tran.y = target.tran.y - tp->currentPos.tran.y;
emcmotStatus->dtg.tran.z = target.tran.z - tp->currentPos.tran.z;
emcmotStatus->dtg.a = target.a - tp->currentPos.a;
emcmotStatus->dtg.b = target.b - tp->currentPos.b;
emcmotStatus->dtg.c = target.c - tp->currentPos.c;
emcmotStatus->dtg.u = target.u - tp->currentPos.u;
emcmotStatus->dtg.v = target.v - tp->currentPos.v;
emcmotStatus->dtg.w = target.w - tp->currentPos.w;
return 0;
}
EmcPose tcGetPosReal(TC_STRUCT * tc, int of_endpoint)
{
EmcPose pos;
PmPose xyz;
PmPose abc;
PmPose uvw;
double progress = of_endpoint? tc->target: tc->progress;
if (tc->motion_type == TC_RIGIDTAP) {
if(tc->coords.rigidtap.state > REVERSING) {
pmLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz);
} else {
pmLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz);
}
// no rotary move allowed while tapping
abc.tran = tc->coords.rigidtap.abc;
uvw.tran = tc->coords.rigidtap.uvw;
} else if (tc->motion_type == TC_LINEAR) {
if (tc->coords.line.xyz.tmag > 0.) {
// progress is along xyz, so uvw and abc move proportionally in order
// to end at the same time.
pmLinePoint(&tc->coords.line.xyz, progress, &xyz);
pmLinePoint(&tc->coords.line.uvw,
progress * tc->coords.line.uvw.tmag / tc->target,
&uvw);
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else if (tc->coords.line.uvw.tmag > 0.) {
// xyz is not moving
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, progress, &uvw);
// abc moves proportionally in order to end at the same time
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else {
// if all else fails, it's along abc only
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, 0.0, &uvw);
pmLinePoint(&tc->coords.line.abc, progress, &abc);
}
} else { //we have TC_CIRCULAR
// progress is always along the xyz circle. This simplification
// is possible since zero-radius arcs are not allowed by the interp.
pmCirclePoint(&tc->coords.circle.xyz,
progress * tc->coords.circle.xyz.angle / tc->target,
&xyz);
// abc moves proportionally in order to end at the same time as the
// circular xyz move.
pmLinePoint(&tc->coords.circle.abc,
progress * tc->coords.circle.abc.tmag / tc->target,
&abc);
// same for uvw
pmLinePoint(&tc->coords.circle.uvw,
progress * tc->coords.circle.uvw.tmag / tc->target,
&uvw);
}
pos.tran = xyz.tran;
pos.a = abc.tran.x;
pos.b = abc.tran.y;
pos.c = abc.tran.z;
pos.u = uvw.tran.x;
pos.v = uvw.tran.y;
pos.w = uvw.tran.z;
return pos;
}
EmcPose tcGetPosReal(TC_STRUCT * tc, int of_endpoint)
{
EmcPose pos;
PmPose xyz;
PmPose abc;
PmPose uvw;
double progress = of_endpoint? tc->target: tc->progress;
#if(TRACE != 0)
static double last_l, last_u,last_x = 0 , last_y = 0, last_z = 0, last_a = 0;
#endif
if (tc->motion_type == TC_RIGIDTAP) {
if(tc->coords.rigidtap.state > REVERSING) {
pmLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz);
} else {
pmLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz);
}
// no rotary move allowed while tapping
abc.tran = tc->coords.rigidtap.abc;
uvw.tran = tc->coords.rigidtap.uvw;
} else if (tc->motion_type == TC_LINEAR) {
if (tc->coords.line.xyz.tmag > 0.) {
// progress is along xyz, so uvw and abc move proportionally in order
// to end at the same time.
pmLinePoint(&tc->coords.line.xyz, progress, &xyz);
pmLinePoint(&tc->coords.line.uvw,
progress * tc->coords.line.uvw.tmag / tc->target,
&uvw);
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else if (tc->coords.line.uvw.tmag > 0.) {
// xyz is not moving
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, progress, &uvw);
// abc moves proportionally in order to end at the same time
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else {
// if all else fails, it's along abc only
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, 0.0, &uvw);
pmLinePoint(&tc->coords.line.abc, progress, &abc);
}
} else if (tc->motion_type == TC_CIRCULAR) {//we have TC_CIRCULAR
// progress is always along the xyz circle. This simplification
// is possible since zero-radius arcs are not allowed by the interp.
pmCirclePoint(&tc->coords.circle.xyz,
progress * tc->coords.circle.xyz.angle / tc->target,
&xyz);
// abc moves proportionally in order to end at the same time as the
// circular xyz move.
pmLinePoint(&tc->coords.circle.abc,
progress * tc->coords.circle.abc.tmag / tc->target,
&abc);
// same for uvw
pmLinePoint(&tc->coords.circle.uvw,
progress * tc->coords.circle.uvw.tmag / tc->target,
&uvw);
} else {
int s, tmp1,i;
double u,*N,R, X, Y, Z, A, B, C, U, V, W, F, D;
double curve_accel;
#if(TRACE != 0)
double delta_l, delta_u, delta_d, delta_x, delta_y, delta_z, delta_a;
#endif
N = tc->nurbs_block.N;
// NL = tc->nurbs_block.NL;
assert(tc->motion_type == TC_NURBS);
u = progress / tc->target;
if (u<1) {
s = nurbs_findspan(tc->nurbs_block.nr_of_ctrl_pts-1, tc->nurbs_block.order - 1,
u, tc->nurbs_block.knots_ptr); //return span index of u_i
nurbs_basisfun(s, u, tc->nurbs_block.order - 1 , tc->nurbs_block.knots_ptr , N); // input: s:knot span index u:u_0 d:B-Spline degree k:Knots
// output: N:basis functions
// refer to bspeval.cc::line(70) of octave
// refer to opennurbs_evaluate_nurbs.cpp::line(985) of openNurbs
// refer to ON_NurbsCurve::Evaluate() for ...
// refer to opennurbs_knot.cpp::ON_NurbsSpanIndex()
// http://www.rhino3d.com/nurbs.htm (What is NURBS?)
// Some modelers that use older algorithms for NURBS
// evaluation require two extra knot values for a total of
// degree+N+1 knots. When Rhino is exporting and importing
// NURBS geometry, it automatically adds and removes these
// two superfluous knots as the situation requires.
tmp1 = s - tc->nurbs_block.order + 1;
assert(tmp1 >= 0);
assert(tmp1 < tc->nurbs_block.nr_of_ctrl_pts);
R = 0.0;
for (i=0; i<=tc->nurbs_block.order -1 ; i++) {
R += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].R;
}
X = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
X += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].X;
}
X = X/R;
xyz.tran.x = X;
Y = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Y += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Y;
}
Y = Y/R;
xyz.tran.y = Y;
Z = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Z += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Z;
}
Z = Z/R;
xyz.tran.z = Z;
A = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
A += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].A;
}
A = A/R;
abc.tran.x = A;
B = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
B += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].B;
}
B = B/R;
abc.tran.y = B;
C = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
C += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].C;
}
C = C/R;
abc.tran.z = C;
U = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
U += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].U;
}
U = U/R;
uvw.tran.x = U;
V = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
V += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].V;
}
V = V/R;
uvw.tran.y = V;
W = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
W += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].W;
}
W = W/R;
uvw.tran.z = W;
F = 0.0;
F = tc->nurbs_block.ctrl_pts_ptr[tmp1].F;
tc->reqvel = F;
D = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
D += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].D;
}
D = D/R;
// compute allowed feed
if(!of_endpoint) {
curve_accel = (tc->cur_vel * tc->cur_vel)/D;
if(curve_accel > tc->maxaccel) {
// modify req_vel
tc->reqvel = pmSqrt((tc->maxaccel * D));
}
}
#if (TRACE != 0)
if(l == 0 && _dt == 0) {
last_l = 0;
last_u = 0;
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = 0;
_dt+=1;
}
delta_l = l - last_l;
last_l = l;
delta_u = u - last_u;
last_u = u;
delta_x = xyz.tran.x - last_x;
delta_y = xyz.tran.y - last_y;
delta_z = xyz.tran.z - last_z;
delta_a = abc.tran.x - last_a;
delta_d = pmSqrt(pmSq(delta_x)+pmSq(delta_y)+pmSq(delta_z));
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = abc.tran.x;
if( delta_d > 0)
{
if(_dt == 1){
/* prepare header for gnuplot */
DPS ("%11s%15s%15s%15s%15s%15s%15s%15s%15s\n",
"#dt", "u", "l","x","y","z","delta_d", "delta_l","a");
}
DPS("%11u%15.10f%15.10f%15.5f%15.5f%15.5f%15.5f%15.5f%15.5f\n",
_dt, u, l,last_x, last_y, last_z, delta_d, delta_l, last_a);
_dt+=1;
}
#endif // (TRACE != 0)
}else {
xyz.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].X;
xyz.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Y;
xyz.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Z;
uvw.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].U;
uvw.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].V;
uvw.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].W;
abc.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].A;
abc.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].B;
abc.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].C;
// R = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].R;
}
}
//DP ("GetEndPoint?(%d) R(%.2f) X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",of_endpoint, R, X, Y, Z, A);
// TODO-eric if R going to show ?
//#if (TRACE != 0)
// if(_dt == 0){
// /* prepare header for gnuplot */
// DPS ("%11s%15s%15s%15s\n", "#dt", "x", "y", "z");
// }
// DPS("%11u%15.5f%15.5f%15.5f\n", _dt, xyz.tran.x, xyz.tran.y, xyz.tran.z);
// _dt+=1;
//#endif // (TRACE != 0)
#if (TRACE != 1)
if( of_endpoint != 1) {
}
#endif
pos.tran = xyz.tran;
pos.a = abc.tran.x;
pos.b = abc.tran.y;
pos.c = abc.tran.z;
pos.u = uvw.tran.x;
pos.v = uvw.tran.y;
pos.w = uvw.tran.z;
// DP ("GetEndPoint?(%d) tc->id %d MotionType %d X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",
// of_endpoint,tc->id,tc->motion_type, pos.tran.x,
// pos.tran.y, pos.tran.z, pos.a);
return pos;
}
