tp.c

Source code for an Numeric Cmputer

                newvel > pmSqrt(tc->maxaccel * tc->coords.circle.xyz.radius))
            newvel = pmSqrt(tc->maxaccel * tc->coords.circle.xyz.radius);

        // 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;
}

// This is the brains of the operation.  It's called every TRAJ period
// and is expected to set tp->currentPos to the new machine position.
// Lots of other tp fields (depth, done, etc) have to be twiddled to
// communicate the status; I think those are spelled out here correctly
// and I can't clean it up without breaking the API that the TP presents
// to motion.  It's not THAT bad and in the interest of not touching
// stuff outside this directory, I'm going to leave it for now.

int tpRunCycle(TP_STRUCT * tp)
{
    // 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 oldrevs, spindleoffset;
    static int waiting = 0;
    double save_vel;

    tc = tcqItem(&tp->queue, 0);
    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;
        tp->synchronized = 0;
        emcmotStatus->spindleSync = 0;
        tpResume(tp);
        return 0;
    }

    if (tc->target == tc->progress) {
        // 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;

        // done with this move
        tcqRemove(&tp->queue, 1);

        // so get next move
        tc = tcqItem(&tp->queue, 0);
        if(!tc) return 0;
    }

    // report our line number to the guis
    tp->execId = tc->id;

    // this is no longer the segment we were waiting for
    if(waiting && waiting != tc->id) 
        waiting = 0;

    if(waiting) {
        double r;
        if((r = emcmotStatus->spindleRevs) >= oldrevs) {
            /* haven't passed index yet */
            oldrevs = r;
            return 0;
        } else {
            /* passed index, start the move */
            emcmotStatus->spindleSync = 1;
            waiting=0;
        }
    }

    if(!tc->synchronized) emcmotStatus->spindleSync = 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(tc->blend_with_next) 
        nexttc = tcqItem(&tp->queue, 1);
    else
        nexttc = NULL;

    if(!tc->synchronized && nexttc && nexttc->synchronized) {
        // 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(tc->active == 0) {
        // this means this tc is being read for the first time.
        
        tc->currentvel = 0;
        tp->depth = tp->activeDepth = 1;
        tp->motionType = tc->canon_motion_type;
        tc->active = 1;
        tc->blending = 0;

        if(tc->synchronized) {
            if(!emcmotStatus->spindleSync) {
                // if we aren't already synced, wait
                waiting = tc->id;
                oldrevs = emcmotStatus->spindleRevs;
                spindleoffset = 0.0;
                // don't move: wait
                return 0;
            }
        }
        // clamp motion's velocity at TRAJ MAX_VELOCITY (tooltip maxvel)
        if(tc->maxvel > tp->vLimit) 
            tc->maxvel = tp->vLimit;

        // honor accel constraint if we happen to make an acute angle
        // with the next segment.  A dot product test could often 
        // eliminate this.
        if(tc->blend_with_next) 
            tc->maxaccel /= 2.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;

        // clamp motion's velocity at TRAJ MAX_VELOCITY (tooltip maxvel)
        if(nexttc->maxvel > tp->vLimit) 
            nexttc->maxvel = tp->vLimit;

        // 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(tp->aborting) {
        // an abort message has come
        if( (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;
            emcmotStatus->spindleSync = 0;
            tpResume(tp);
            return 0;
        } else {
            tc->reqvel = 0.0;
            if(nexttc) nexttc->reqvel = 0.0;
        }
    }


    if(tc->synchronized) {
        double pos_error;
        double revs = emcmotStatus->spindleRevs;

        pos_error = (revs - spindleoffset) * tc->uu_per_rev - (tc->progress + nexttc->progress);

        tc->reqvel = pos_error/tc->cycle_time/2.0;
        tc->feed_override = 1.0;
        if(tc->reqvel < 0.0) tc->reqvel = 0.0;
        if(nexttc && nexttc->synchronized) {
            nexttc->reqvel = pos_error/nexttc->cycle_time;
            nexttc->feed_override = 1.0;
            if(nexttc->reqvel < 0.0) nexttc->reqvel = 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(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;

    // blend criteria
    if(tc->blending || 
            (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)
            tp->motionType = tc->canon_motion_type;
        else
            tp->motionType = nexttc->canon_motion_type;

        secondary_before = tcGetPos(nexttc);
        save_vel = nexttc->reqvel;
        nexttc->reqvel = (tc->vel_at_blend_start - primary_vel) / nexttc->feed_override;
        tcRunCycle(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;

        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;
    } else {
        tp->motionType = tc->canon_motion_type;
        tp->currentPos = primary_after;
    }
    return 0;
}

int tpSetSpindleSync(TP_STRUCT * tp, double sync) {
    if(sync) {
        tp->synchronized = 1;
        tp->uu_per_rev = sync;
    } else
        tp->synchronized = 0;

    return 0;
}

int tpPause(TP_STRUCT * tp)
{
    if (0 == tp) {
	return -1;
    }

    if (!tp->pausing) {
	/* save the restore value */
	tp->vRestore = tp->vScale;

	/* apply 0 scale to queued motions */
	tpSetVscale(tp, 0);

	/* mark us pausing-- do this after the call to toSetVscale since this
	   looks at the pausing flag to decide whether to set vRestore (if
	   pausing) or vScale (if not). We want vScale to be set in the call
	   for this one */
	tp->pausing = 1;
    }

    return 0;
}

int tpResume(TP_STRUCT * tp)
{
    if (0 == tp) {
	return -1;
    }

    if (tp->pausing) {
	/* mark us not pausing-- this must be done before the call to
	   tpSetVscale since that function will only apply the restored scale
	   value if we're not pausing */
	tp->pausing = 0;

	/* restore scale value */
	tp->vScale = tp->vRestore;

	/* apply the restored scale value to queued motions */
	tpSetVscale(tp, tp->vScale);
    }

    return 0;
}

int tpAbort(TP_STRUCT * tp)
{
    if (0 == tp) {
	return -1;
    }

    if (!tp->aborting) {
	/* to abort, signal a pause and set our abort flag */
	tpPause(tp);
	tp->aborting = 1;
    }
    return 0;
}

int tpGetMotionType(TP_STRUCT * tp)
{
    return tp->motionType;
}

EmcPose tpGetPos(TP_STRUCT * tp)
{
    EmcPose retval;

    if (0 == tp) {
	retval.tran.x = retval.tran.y = retval.tran.z = 0.0;
	retval.a = retval.b = retval.c = 0.0;
	return retval;
    }

    return tp->currentPos;
}

int tpIsDone(TP_STRUCT * tp)
{
    if (0 == tp) {
	return 0;
    }

    return tp->done;
}

/*
  tpIsPaused() returns 1 only when all active queued motions are paused.
  This is necessary so that abort clears the queue when motions have really
  stopped. If there are no queued motions, it returns 1 if the pausing
  flag is set. */
int tpIsPaused(TP_STRUCT * tp)
{
    int t;

    if (0 == tp) {
	return 0;
    }

    if (0 == tp->depth) {
	return tp->pausing;
    }

    for (t = 0; t < tp->activeDepth; t++) {
        TC_STRUCT *tc;
        tc = tcqItem(&tp->queue, t);
        if (tc->feed_override != 0) {
	    return 0;
	}
    }

    return 1;
}

int tpQueueDepth(TP_STRUCT * tp)
{
    if (0 == tp) {
	return 0;
    }

    return tp->depth;
}

int tpActiveDepth(TP_STRUCT * tp)
{
    if (0 == tp) {
	return 0;
    }

    return tp->activeDepth;
}

int tpSetAout(TP_STRUCT *tp, unsigned char index, double start, double end) {
    return 0;
}

int tpSetDout(TP_STRUCT *tp, int index, unsigned char start, unsigned char end) {
    return 0;
}