PmCartesian tcGetStartingUnitVector(TC_STRUCT *tc) {
PmCartesian v;
if(tc->motion_type == TC_LINEAR || tc->motion_type == TC_RIGIDTAP) {
pmCartCartSub(tc->coords.line.xyz.end.tran, tc->coords.line.xyz.start.tran, &v);
} else {
PmPose startpoint;
PmCartesian radius;
PmCartesian tan, perp;
pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint);
pmCartCartSub(startpoint.tran, tc->coords.circle.xyz.center, &radius);
pmCartCartCross(tc->coords.circle.xyz.normal, radius, &tan);
pmCartUnit(tan, &tan);
pmCartCartSub(tc->coords.circle.xyz.center, startpoint.tran, &perp);
pmCartUnit(perp, &perp);
pmCartScalMult(tan, tc->maxaccel, &tan);
pmCartScalMult(perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius, &perp);
pmCartCartAdd(tan, perp, &v);
}
pmCartUnit(v, &v);
return v;
}
PmCartesian tcGetUnitCart(TC_STRUCT *tc)
{
PmPose currentPose;
PmCartesian radialCart;
static const PmCartesian fake= {1.0,0.0,0.0};
if(tc->type == TC_LINEAR)
{
pmCartCartSub(tc->line.end.tran,tc->line.start.tran,&tc->unitCart);
#ifdef USE_PM_CART_NORM
pmCartNorm(tc->unitCart,&tc->unitCart);
#else
pmCartUnit(tc->unitCart,&tc->unitCart);
#endif
return(tc->unitCart);
}
else if(tc->type == TC_CIRCULAR)
{
pmCirclePoint(&tc->circle,tc->currentPos,¤tPose);
pmCartCartSub(currentPose.tran, tc->circle.center, &radialCart);
pmCartCartCross(tc->circle.normal, radialCart,&tc->unitCart);
#ifdef USE_PM_CART_NORM
pmCartNorm(tc->unitCart,&tc->unitCart);
#else
pmCartUnit(tc->unitCart,&tc->unitCart);
#endif
return(tc->unitCart);
}
// It should never really get here.
return fake;
}
int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end)
{
if (!circ || new_angle <= DOUBLE_FUZZ) {
return PM_ERR;
}
double mag = 0;
pmCartMagSq(&circ->rHelix, &mag);
if ( mag > 1e-6 ) {
//Can't handle helices
return PM_ERR;
}
//TODO handle spiral?
if (from_end) {
//Not implemented yet, way more reprocessing...
PmCartesian new_start;
double start_angle = circ->angle - new_angle;
pmCirclePoint(circ, start_angle, &new_start);
pmCartCartSub(&new_start, &circ->center, &circ->rTan);
pmCartCartCross(&circ->normal, &circ->rTan, &circ->rPerp);
pmCartMag(&circ->rTan, &circ->radius);
}
//Reduce the spiral proportionally
circ->spiral *= (new_angle / circ->angle);
// Easy to grow / shrink from start
circ->angle = new_angle;
return 0;
}
int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end)
{
int r1 = 0, r2 = 0;
double tmag = 0.0;
if (0 == line) {
return (pmErrno = PM_ERR);
}
line->start = *start;
line->end = *end;
r1 = pmCartCartSub(end, start, &line->uVec);
if (r1) {
return r1;
}
pmCartMag(&line->uVec, &tmag);
if (IS_FUZZ(tmag, CART_FUZZ)) {
line->uVec.x = 1.0;
line->uVec.y = 0.0;
line->uVec.z = 0.0;
} else {
r2 = pmCartUnit(&line->uVec, &line->uVec);
}
line->tmag = tmag;
line->tmag_zero = (line->tmag <= CART_FUZZ);
/* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
}
int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, PmCartesian * const vout)
{
int r1, r2;
PmCartesian par;
r1 = pmCartCartProj(v, normal, &par);
r2 = pmCartCartSub(v, &par, vout);
return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}
PmCartesian tcGetEndingUnitVector(TC_STRUCT *tc) {
PmCartesian v;
if(tc->motion_type == TC_LINEAR) {
pmCartCartSub(tc->coords.line.xyz.end.tran, tc->coords.line.xyz.start.tran, &v);
} else if(tc->motion_type == TC_RIGIDTAP) {
// comes out the other way
pmCartCartSub(tc->coords.line.xyz.start.tran, tc->coords.line.xyz.end.tran, &v);
} else {
PmPose endpoint;
PmCartesian radius;
pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint);
pmCartCartSub(endpoint.tran, tc->coords.circle.xyz.center, &radius);
pmCartCartCross(tc->coords.circle.xyz.normal, radius, &v);
}
pmCartUnit(v, &v);
return v;
}
int tcCircleEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out)
{
PmCartesian endpoint;
PmCartesian radius;
pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint);
pmCartCartSub(&endpoint, &tc->coords.circle.xyz.center, &radius);
pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, out);
pmCartUnitEq(out);
return 0;
}
int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end)
{
int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
double tmag = 0.0;
double rmag = 0.0;
PmQuaternion startQuatInverse;
if (0 == line) {
return (pmErrno = PM_ERR);
}
r3 = pmQuatInv(&start->rot, &startQuatInverse);
if (r3) {
return r3;
}
r4 = pmQuatQuatMult(&startQuatInverse, &end->rot, &line->qVec);
if (r4) {
return r4;
}
pmQuatMag(&line->qVec, &rmag);
if (rmag > Q_FUZZ) {
r5 = pmQuatScalMult(&line->qVec, 1 / rmag, &(line->qVec));
if (r5) {
return r5;
}
}
line->start = *start;
line->end = *end;
r1 = pmCartCartSub(&end->tran, &start->tran, &line->uVec);
if (r1) {
return r1;
}
pmCartMag(&line->uVec, &tmag);
if (IS_FUZZ(tmag, CART_FUZZ)) {
line->uVec.x = 1.0;
line->uVec.y = 0.0;
line->uVec.z = 0.0;
} else {
r2 = pmCartUnit(&line->uVec, &line->uVec);
}
line->tmag = tmag;
line->rmag = rmag;
line->tmag_zero = (line->tmag <= CART_FUZZ);
line->rmag_zero = (line->rmag <= Q_FUZZ);
/* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
}
int arcConvexTest(PmCartesian const * const center,
PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir)
{
//Check if an arc-line intersection is concave or convex
double dot;
PmCartesian diff;
pmCartCartSub(P, center, &diff);
pmCartCartDot(&diff, uVec, &dot);
tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir);
int convex = (reverse_dir != 0) ^ (dot < 0);
return convex;
}
int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out)
{
PmCartesian startpoint;
PmCartesian radius;
PmCartesian tan, perp;
pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint);
pmCartCartSub(&startpoint, &tc->coords.circle.xyz.center, &radius);
pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, &tan);
pmCartUnitEq(&tan);
//The unit vector's actual direction is adjusted by the normal
//acceleration here. This unit vector is NOT simply the tangent
//direction.
pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp);
pmCartUnitEq(&perp);
pmCartScalMult(&tan, tc->maxaccel, &tan);
pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius);
pmCartCartAdd(&tan, &perp, out);
pmCartUnitEq(out);
return 0;
}
int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1,
PmCartLine const * const line2, double radius,
double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume) {
PmCartesian center, normal, binormal;
// Pointer to middle point of line segment pair
PmCartesian const * const middle = &line1->end;
//TODO assert line1 end = line2 start?
//Calculate the normal direction of the arc from the difference
//between the unit vectors
pmCartCartSub(&line2->uVec, &line1->uVec, &normal);
pmCartUnitEq(&normal);
pmCartScalMultEq(&normal, center_dist);
pmCartCartAdd(middle, &normal, ¢er);
//Calculate the binormal (vector perpendicular to the plane of the
//arc)
pmCartCartCross(&line1->uVec, &line2->uVec, &binormal);
pmCartUnitEq(&binormal);
// Start point is blend_dist away from middle point in the
// negative direction of line1
pmCartScalMult(&line1->uVec, -blend_dist, start);
pmCartCartAdd(start, middle, start);
// End point is blend_dist away from middle point in the positive
// direction of line2
pmCartScalMult(&line2->uVec, blend_dist, end);
pmCartCartAddEq(end, middle);
//Handle line portion of line-arc
arc->uTan = line1->uVec;
if (consume) {
arc->line_length = line1->tmag - blend_dist;
} else {
arc->line_length = 0;
}
return arcInitFromPoints(arc, start, end, ¢er);
}
/**
* Find the geometric tangent vector to a helical arc.
* Unlike the acceleration vector, the result of this calculation is a vector
* tangent to the helical arc. This is called by wrapper functions for the case of a circular or helical arc.
*/
int pmCircleTangentVector(PmCircle const * const circle,
double angle_in, PmCartesian * const out)
{
PmCartesian startpoint;
PmCartesian radius;
PmCartesian uTan, dHelix, dRadial;
// Get vector in radial direction
pmCirclePoint(circle, angle_in, &startpoint);
pmCartCartSub(&startpoint, &circle->center, &radius);
/* Find local tangent vector using planar normal. Assuming a differential
* angle dtheta, the tangential component of the tangent vector is r *
* dtheta. Since we're normalizing the vector anyway, assume dtheta = 1.
*/
pmCartCartCross(&circle->normal, &radius, &uTan);
// find dz/dtheta and get differential movement along helical axis
double h;
pmCartMag(&circle->rHelix, &h);
/* the binormal component of the tangent vector is (dz / dtheta) * dtheta.
*/
double dz = 1.0 / circle->angle;
pmCartScalMult(&circle->rHelix, dz, &dHelix);
pmCartCartAddEq(&uTan, &dHelix);
/* The normal component is (dr / dtheta) * dtheta.
*/
double dr = circle->spiral / circle->angle;
pmCartUnit(&radius, &dRadial);
pmCartScalMultEq(&dRadial, dr);
pmCartCartAddEq(&uTan, &dRadial);
//Normalize final output vector
pmCartUnit(&uTan, out);
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;
}
/*
pmCircleInit() takes the defining parameters of a generalized circle
and sticks them in the structure. It also computes the radius and vectors
in the plane that are useful for other functions and that don't need
to be recomputed every time.
Note that the end can be placed arbitrarily, resulting in a combination of
spiral and helical motion. There is an overconstraint between the start,
center, and normal vector: the center vector and start vector are assumed
to be in the plane defined by the normal vector. If this is not true, then
it will be made true by moving the center vector onto the plane.
*/
int pmCircleInit(PmCircle * const circle,
PmCartesian const * const start, PmCartesian const * const end,
PmCartesian const * const center, PmCartesian const * const normal, int turn)
{
double dot;
PmCartesian rEnd;
PmCartesian v;
double d;
int r1;
#ifdef PM_DEBUG
if (0 == circle) {
#ifdef PM_PRINT_ERROR
pmPrintError("error: pmCircleInit cirle pointer is null\n");
#endif
return pmErrno = PM_ERR;
}
#endif
/* adjust center */
pmCartCartSub(start, center, &v);
r1 = pmCartCartProj(&v, normal, &v);
if (PM_NORM_ERR == r1) {
/* bad normal vector-- abort */
#ifdef PM_PRINT_ERROR
pmPrintError("error: pmCircleInit normal vector is 0\n");
#endif
return -1;
}
pmCartCartAdd(&v, center, &circle->center);
/* normalize and redirect normal vector based on turns. If turn is less
than 0, point normal vector in other direction and make turn positive,
-1 -> 0, -2 -> 1, etc. */
pmCartUnit(normal, &circle->normal);
if (turn < 0) {
turn = -1 - turn;
pmCartScalMult(&circle->normal, -1.0, &circle->normal);
}
/* radius */
pmCartCartDisp(start, &circle->center, &circle->radius);
/* vector in plane of circle from center to start, magnitude radius */
pmCartCartSub(start, &circle->center, &circle->rTan);
/* vector in plane of circle perpendicular to rTan, magnitude radius */
pmCartCartCross(&circle->normal, &circle->rTan, &circle->rPerp);
/* do rHelix, rEnd */
pmCartCartSub(end, &circle->center, &circle->rHelix);
pmCartPlaneProj(&circle->rHelix, &circle->normal, &rEnd);
pmCartMag(&rEnd, &circle->spiral);
circle->spiral -= circle->radius;
pmCartCartSub(&circle->rHelix, &rEnd, &circle->rHelix);
pmCartUnit(&rEnd, &rEnd);
pmCartScalMult(&rEnd, circle->radius, &rEnd);
/* Patch for error spiral end same as spiral center */
pmCartMag(&rEnd, &d);
if (d == 0.0) {
pmCartScalMult(&circle->normal, DOUBLE_FUZZ, &v);
pmCartCartAdd(&rEnd, &v, &rEnd);
}
/* end patch 03-mar-1999 Dirk Maij */
/* angle */
pmCartCartDot(&circle->rTan, &rEnd, &dot);
dot = dot / (circle->radius * circle->radius);
if (dot > 1.0) {
circle->angle = 0.0;
} else if (dot < -1.0) {
circle->angle = PM_PI;
} else {
circle->angle = rtapi_acos(dot);
}
/* now angle is in range 0..PI . Check if cross is antiparallel to
normal. If so, true angle is between PI..2PI. Need to subtract from
2PI. */
pmCartCartCross(&circle->rTan, &rEnd, &v);
pmCartCartDot(&v, &circle->normal, &d);
if (d < 0.0) {
circle->angle = PM_2_PI - circle->angle;
}
if (circle->angle > -(CIRCLE_FUZZ) && circle->angle < (CIRCLE_FUZZ)) {
circle->angle = PM_2_PI;
}
/* now add more angle for multi turns */
if (turn > 0) {
circle->angle += turn * 2.0 * PM_PI;
}
//Default to invalid
/* if 0'ed out while not debugging*/
#if 0
printf("\n\n");
printf("pmCircleInit:\n");
printf(" \t start : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
start->x, start->y, start->z);
printf(" \t end : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
end->x, end->y, end->z);
printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
center->x, center->y, center->z);
printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
normal->x, normal->y, normal->z);
printf(" \t rEnd : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
rEnd.x, rEnd.y, rEnd.z);
printf(" \t turn=%d\n", turn);
printf(" \t dot=%9.9f\n", dot);
printf(" \t d=%9.9f\n", d);
printf(" \t circle \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n",
circle->angle, circle->radius, circle->spiral);
printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
circle->normal.x, circle->normal.y, circle->normal.z);
printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
circle->center.x, circle->center.y, circle->center.z);
printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
circle->rTan.x, circle->rTan.y, circle->rTan.z);
printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
circle->rPerp.x, circle->rPerp.y, circle->rPerp.z);
printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
circle->rHelix.x, circle->rHelix.y, circle->rHelix.z);
printf("\n\n");
#endif
return pmErrno = 0;
}
int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
PmCartesian const * const end,
PmCartesian const * const center)
{
#ifdef ARC_PEDANTIC
if (!P0 || !P1 || !center) {
return TP_ERR_MISSING_INPUT;
if (!arc) {
return TP_ERR_MISSING_OUTPUT;
}
#endif
// Store the start, end, and center
arc->start = *start;
arc->end = *end;
arc->center = *center;
pmCartCartSub(start, center, &arc->rStart);
pmCartCartSub(end, center, &arc->rEnd);
// Find the radii at start and end. These are identical for a perfect spherical arc
double radius0, radius1;
pmCartMag(&arc->rStart, &radius0);
pmCartMag(&arc->rEnd, &radius1);
tp_debug_print("radii are %g and %g\n",
radius0,
radius1);
if (radius0 < ARC_MIN_RADIUS || radius1 < ARC_MIN_RADIUS) {
tp_debug_print("radius below min radius %f, aborting arc\n",
ARC_MIN_RADIUS);
return TP_ERR_RADIUS;
}
// Choose initial radius as nominal radius
arc->radius = radius0;
// Get unit vectors from center to start and center to end
PmCartesian u0, u1;
pmCartScalMult(&arc->rStart, 1.0 / radius0, &u0);
pmCartScalMult(&arc->rEnd, 1.0 / radius1, &u1);
// Find arc angle
double dot;
pmCartCartDot(&u0, &u1, &dot);
arc->angle = acos(dot);
tp_debug_print("spherical arc angle = %f\n", arc->angle);
// Store spiral factor as radial difference. Archimedean spiral coef. a = spiral / angle
arc->spiral = (radius1 - radius0 );
if (arc->angle < ARC_MIN_ANGLE) {
tp_debug_print("angle %f below min angle %f, aborting arc\n",
arc->angle,
ARC_MIN_ANGLE);
return TP_ERR_GEOM;
}
// Store sin of arc angle since it is reused many times for SLERP
arc->Sangle = sin(arc->angle);
return TP_ERR_OK;
}
int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out)
{
//TODO pedantic
//Convert progress to actual progress around the arc
double net_progress = progress - arc->line_length;
if (net_progress <= 0.0 && arc->line_length > 0) {
tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length);
//Get position on line (not actually an angle in this case)
pmCartScalMult(&arc->uTan, net_progress, out);
pmCartCartAdd(out, &arc->start, out);
} else {
double angle_in = net_progress / arc->radius;
tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle);
double scale0 = sin(arc->angle - angle_in) / arc->Sangle;
double scale1 = sin(angle_in) / arc->Sangle;
PmCartesian interp0,interp1;
pmCartScalMult(&arc->rStart, scale0, &interp0);
pmCartScalMult(&arc->rEnd, scale1, &interp1);
pmCartCartAdd(&interp0, &interp1, out);
pmCartCartAdd(&arc->center, out, out);
}
return TP_ERR_OK;
}