int pmLinePoint(PmLine const * const line, double len, PmPose * const point)
{
int r1 = 0, r2 = 0, r3 = 0, r4 = 0;
if (line->tmag_zero) {
point->tran = line->end.tran;
} else {
/* return start + len * uVec */
r1 = pmCartScalMult(&line->uVec, len, &point->tran);
r2 = pmCartCartAdd(&line->start.tran, &point->tran, &point->tran);
}
if (line->rmag_zero) {
point->rot = line->end.rot;
} else {
if (line->tmag_zero) {
r3 = pmQuatScalMult(&line->qVec, len, &point->rot);
} else {
r3 = pmQuatScalMult(&line->qVec, len * line->rmag / line->tmag,
&point->rot);
}
r4 = pmQuatQuatMult(&line->start.rot, &point->rot, &point->rot);
}
return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : 0;
}
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;
}
int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end)
{
PmCartesian r_perp;
PmCartesian r_tan;
if (at_end) {
r_perp = arc->rEnd;
} else {
r_perp = arc->rStart;
}
pmCartCartCross(&arc->binormal, &r_perp, &r_tan);
//Get spiral component
double dr = arc->spiral / arc->angle;
//Get perpendicular component due to spiral
PmCartesian d_perp;
pmCartUnit(&r_perp, &d_perp);
pmCartScalMultEq(&d_perp, dr);
//TODO error checks
pmCartCartAdd(&d_perp, &r_tan, tan);
pmCartUnitEq(tan);
return TP_ERR_OK;
}
/*
pmCirclePoint() returns the point at the given angle along
the circle. If the circle is a helix or spiral or combination, the
point will include interpolation off the actual circle.
*/
int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point)
{
PmCartesian par, perp;
double scale;
#ifdef PM_DEBUG
if (0 == circle || 0 == point) {
#ifdef PM_PRINT_ERROR
pmPrintError
("error: pmCirclePoint circle or point pointer is null\n");
#endif
return pmErrno = PM_ERR;
}
#endif
/* compute components rel to center */
pmCartScalMult(&circle->rTan, rtapi_cos(angle), &par);
pmCartScalMult(&circle->rPerp, rtapi_sin(angle), &perp);
/* add to get radius vector rel to center */
pmCartCartAdd(&par, &perp, point);
/* get scale for spiral, helix interpolation */
if (circle->angle == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError("error: pmCirclePoint angle is zero\n");
#endif
return pmErrno = PM_DIV_ERR;
}
scale = angle / circle->angle;
/* add scaled vector in radial dir for spiral */
pmCartUnit(point, &par);
pmCartScalMult(&par, scale * circle->spiral, &par);
pmCartCartAdd(point, &par, point);
/* add scaled vector in helix dir */
pmCartScalMult(&circle->rHelix, scale, &perp);
pmCartCartAdd(point, &perp, point);
/* add to center vector for final result */
pmCartCartAdd(&circle->center, point, point);
return pmErrno = 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);
}
int pmCartLinePoint(PmCartLine const * const line, double len, PmCartesian * const point)
{
int r1 = 0, r2 = 0;
if (line->tmag_zero) {
*point = line->end;
} else {
/* return start + len * uVec */
r1 = pmCartScalMult(&line->uVec, len, point);
r2 = pmCartCartAdd(&line->start, point, point);
}
return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
}
int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCartesian * const vout)
{
int r1, r2;
#ifdef PM_DEBUG
if (!pmQuatIsNorm(&p1->rot)) {
#ifdef PM_PRINT_ERROR
pmPrintError(&"Bad quaternion in pmPoseCartMult\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
r1 = pmQuatCartMult(&p1->rot, v2, vout);
r2 = pmCartCartAdd(&p1->tran, vout, vout);
return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
}
int pmPosePoseMult(PmPose const * const p1, PmPose const * const p2, PmPose * const pout)
{
int r1, r2, r3;
#ifdef PM_DEBUG
if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad quaternion in pmPosePoseMult\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
r1 = pmQuatCartMult(&p1->rot, &p2->tran, &pout->tran);
r2 = pmCartCartAdd(&p1->tran, &pout->tran, &pout->tran);
r3 = pmQuatQuatMult(&p1->rot, &p2->rot, &pout->rot);
return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
}
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 pmCartLineStretch(PmCartLine * const line, double new_len, int from_end)
{
int r1 = 0, r2 = 0;
if (!line || line->tmag_zero || new_len <= DOUBLE_FUZZ) {
return PM_ERR;
}
if (from_end) {
// Store the new relative position from end in the start point
r1 = pmCartScalMult(&line->uVec, -new_len, &line->start);
// Offset the new start point by the current end point
r2 = pmCartCartAddEq(&line->start, &line->end);
} else {
// Store the new relative position from start in the end point:
r1 = pmCartScalMult(&line->uVec, new_len, &line->end);
// Offset the new end point by the current start point
r2 = pmCartCartAdd(&line->start, &line->end, &line->end);
}
line->tmag = new_len;
return pmErrno = (r1 || r2) ? PM_NORM_ERR : 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;
}
