int tcInit(TC_STRUCT *tc) { PmPose zero; if (0 == tc) { return -1; } tc->cycleTime = 0.0; tc->targetPos = 0.0; tc->vMax = 0.0; tc->vScale = 1.0; tc->aMax = 0.0; tc->preVMax = 0.0; tc->preAMax = 0.0; tc->vLimit = 0.0; tc->toGo = 0.0; tc->currentPos = 0.0; tc->currentVel = 0.0; tc->currentAccel = 0.0; tc->tcFlag = TC_IS_UNSET; tc->type = TC_LINEAR; /* default is linear interpolation */ tc->id = 0; tc->termCond = TC_TERM_COND_BLEND; tc->tmag=0.0; tc->abc_mag=0.0; tc->tvMax=0.0; tc->taMax=0.0; tc->abc_vMax=0.0; tc->abc_aMax=0.0; tc->unitCart.x = tc->unitCart.y = tc->unitCart.z = 0.0; zero.tran.x = zero.tran.y = zero.tran.z = 0.0; zero.rot.s=1.0; zero.rot.x = zero.rot.y = zero.rot.z = 0.0; pmLineInit(&tc->line, zero, zero); pmLineInit(&tc->line_abc, zero, zero); /* since type is TC_LINEAR, don't need to set circle params */ tc->douts = 0; tc->doutstarts = 0; tc->doutends = 0; return 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; }
int tpAddCircle(TP_STRUCT * tp, EmcPose end, PmCartesian center, PmCartesian normal, int turn, int type, double vel, double ini_maxvel, double acc, unsigned char enables, char atspeed) { TC_STRUCT tc; PmCircle circle; PmLine line_uvw, line_abc; PmPose start_xyz, end_xyz; PmPose start_uvw, end_uvw; PmPose start_abc, end_abc; double helix_z_component; // z of the helix's cylindrical coord system double helix_length; PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 }; if (!tp || tp->aborting) return -1; start_xyz.tran = tp->goalPos.tran; end_xyz.tran = end.tran; start_abc.tran.x = tp->goalPos.a; start_abc.tran.y = tp->goalPos.b; start_abc.tran.z = tp->goalPos.c; end_abc.tran.x = end.a; end_abc.tran.y = end.b; end_abc.tran.z = end.c; start_uvw.tran.x = tp->goalPos.u; start_uvw.tran.y = tp->goalPos.v; start_uvw.tran.z = tp->goalPos.w; end_uvw.tran.x = end.u; end_uvw.tran.y = end.v; end_uvw.tran.z = end.w; start_xyz.rot = identity_quat; end_xyz.rot = identity_quat; start_uvw.rot = identity_quat; end_uvw.rot = identity_quat; start_abc.rot = identity_quat; end_abc.rot = identity_quat; pmCircleInit(&circle, start_xyz, end_xyz, center, normal, turn); pmLineInit(&line_uvw, start_uvw, end_uvw); pmLineInit(&line_abc, start_abc, end_abc); // find helix length pmCartMag(circle.rHelix, &helix_z_component); helix_length = pmSqrt(pmSq(circle.angle * circle.radius) + pmSq(helix_z_component)); tc.sync_accel = 0; tc.cycle_time = tp->cycleTime; tc.target = helix_length; tc.progress = 0.0; tc.reqvel = vel; tc.maxaccel = acc; tc.feed_override = 0.0; tc.maxvel = ini_maxvel; tc.id = tp->nextId; tc.active = 0; tc.atspeed = atspeed; tc.currentvel = 0.0; tc.blending = 0; tc.blend_vel = 0.0; tc.vel_at_blend_start = 0.0; tc.coords.circle.xyz = circle; tc.coords.circle.uvw = line_uvw; tc.coords.circle.abc = line_abc; tc.motion_type = TC_CIRCULAR; tc.canon_motion_type = type; tc.blend_with_next = tp->termCond == TC_TERM_COND_BLEND; tc.tolerance = tp->tolerance; tc.synchronized = tp->synchronized; tc.velocity_mode = tp->velocity_mode; tc.uu_per_rev = tp->uu_per_rev; tc.enables = enables; tc.indexrotary = -1; if (syncdio.anychanged != 0) { tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it } else { tc.syncdio.anychanged = 0; } if (tcqPut(&tp->queue, tc) == -1) { return -1; } tp->goalPos = end; tp->done = 0; tp->depth = tcqLen(&tp->queue); tp->nextId++; return 0; }
int tpAddLine(TP_STRUCT * tp, EmcPose end, int type, double vel, double ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary) { TC_STRUCT tc; PmLine line_xyz, line_uvw, line_abc; PmPose start_xyz, end_xyz; PmPose start_uvw, end_uvw; PmPose start_abc, end_abc; PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 }; if (!tp) { rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n"); return -1; } if (tp->aborting) { rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); return -1; } start_xyz.tran = tp->goalPos.tran; end_xyz.tran = end.tran; start_uvw.tran.x = tp->goalPos.u; start_uvw.tran.y = tp->goalPos.v; start_uvw.tran.z = tp->goalPos.w; end_uvw.tran.x = end.u; end_uvw.tran.y = end.v; end_uvw.tran.z = end.w; start_abc.tran.x = tp->goalPos.a; start_abc.tran.y = tp->goalPos.b; start_abc.tran.z = tp->goalPos.c; end_abc.tran.x = end.a; end_abc.tran.y = end.b; end_abc.tran.z = end.c; start_xyz.rot = identity_quat; end_xyz.rot = identity_quat; start_uvw.rot = identity_quat; end_uvw.rot = identity_quat; start_abc.rot = identity_quat; end_abc.rot = identity_quat; pmLineInit(&line_xyz, start_xyz, end_xyz); pmLineInit(&line_uvw, start_uvw, end_uvw); pmLineInit(&line_abc, start_abc, end_abc); tc.sync_accel = 0; tc.cycle_time = tp->cycleTime; if (!line_xyz.tmag_zero) tc.target = line_xyz.tmag; else if (!line_uvw.tmag_zero) tc.target = line_uvw.tmag; else tc.target = line_abc.tmag; tc.progress = 0.0; tc.reqvel = vel; tc.maxaccel = acc; tc.feed_override = 0.0; tc.maxvel = ini_maxvel; tc.id = tp->nextId; tc.active = 0; tc.atspeed = atspeed; tc.currentvel = 0.0; tc.blending = 0; tc.blend_vel = 0.0; tc.vel_at_blend_start = 0.0; tc.coords.line.xyz = line_xyz; tc.coords.line.uvw = line_uvw; tc.coords.line.abc = line_abc; tc.motion_type = TC_LINEAR; tc.canon_motion_type = type; tc.blend_with_next = tp->termCond == TC_TERM_COND_BLEND; tc.tolerance = tp->tolerance; tc.synchronized = tp->synchronized; tc.velocity_mode = tp->velocity_mode; tc.uu_per_rev = tp->uu_per_rev; tc.enables = enables; tc.indexrotary = indexrotary; if (syncdio.anychanged != 0) { tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it } else { tc.syncdio.anychanged = 0; } if (tcqPut(&tp->queue, tc) == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n"); return -1; } tp->goalPos = end; // remember the end of this move, as it's // the start of the next one. tp->done = 0; tp->depth = tcqLen(&tp->queue); tp->nextId++; return 0; }
int tpAddRigidTap(TP_STRUCT *tp, EmcPose end, double vel, double ini_maxvel, double acc, unsigned char enables) { TC_STRUCT tc; PmLine line_xyz; PmPose start_xyz, end_xyz; PmCartesian abc, uvw; PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 }; if (!tp) { rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n"); return -1; } if (tp->aborting) { rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); return -1; } start_xyz.tran = tp->goalPos.tran; end_xyz.tran = end.tran; start_xyz.rot = identity_quat; end_xyz.rot = identity_quat; // abc cannot move abc.x = tp->goalPos.a; abc.y = tp->goalPos.b; abc.z = tp->goalPos.c; uvw.x = tp->goalPos.u; uvw.y = tp->goalPos.v; uvw.z = tp->goalPos.w; pmLineInit(&line_xyz, start_xyz, end_xyz); tc.sync_accel = 0; tc.cycle_time = tp->cycleTime; tc.coords.rigidtap.reversal_target = line_xyz.tmag; // allow 10 turns of the spindle to stop - we don't want to just go on forever tc.target = line_xyz.tmag + 10. * tp->uu_per_rev; tc.progress = 0.0; tc.reqvel = vel; tc.maxaccel = acc; tc.feed_override = 0.0; tc.maxvel = ini_maxvel; tc.id = tp->nextId; tc.active = 0; tc.atspeed = 1; tc.currentvel = 0.0; tc.blending = 0; tc.blend_vel = 0.0; tc.vel_at_blend_start = 0.0; tc.coords.rigidtap.xyz = line_xyz; tc.coords.rigidtap.abc = abc; tc.coords.rigidtap.uvw = uvw; tc.coords.rigidtap.state = TAPPING; tc.motion_type = TC_RIGIDTAP; tc.canon_motion_type = 0; tc.blend_with_next = 0; tc.tolerance = tp->tolerance; if(!tp->synchronized) { rtapi_print_msg(RTAPI_MSG_ERR, "Cannot add unsynchronized rigid tap move.\n"); return -1; } tc.synchronized = tp->synchronized; tc.uu_per_rev = tp->uu_per_rev; tc.velocity_mode = tp->velocity_mode; tc.enables = enables; tc.indexrotary = -1; if (syncdio.anychanged != 0) { tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it } else { tc.syncdio.anychanged = 0; } if (tcqPut(&tp->queue, tc) == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n"); return -1; } // do not change tp->goalPos here, // since this move will end just where it started tp->done = 0; tp->depth = tcqLen(&tp->queue); tp->nextId++; return 0; }