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