/* look at the last item in the list, without removing */
TC_STRUCT * tcqLast(TC_QUEUE_STRUCT *tcq, int *status)
{
if (0 == tcq)
{
return (TC_STRUCT *) 0;
}
return tcqItem(tcq, tcq->_len - 1, status);
}
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;
}
