Beispiel #1
0
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;
}
Beispiel #2
0
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;
}
Beispiel #4
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(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;
}