jepler/linuxcnc
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

use full acceleration when the angle between successive segments is known to be nonacute

Browse source
This commit is contained in:
Jeff Epler 2006-06-14 16:30:18 +00:00
parent 791ec6150a
commit b5e1ba20e3
1 changed files with 41 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

47
src/emc/kinematics/tp.c
View file

@ -436,6 +436,38 @@ void tcRunCycle(TC_STRUCT *tc, double *v, int *on_final_decel) {
if(on_final_decel) *on_final_decel = fabs(maxnewvel - newvel) < 0.001;
}
// Return 1 if the "angle" between the TC elements is acute. It also returns 1
// in some "hard to tell" cases, which is the safe thing to do because
// acceleration is cut in half whenever acute() is true.
int acute(TC_STRUCT *a, TC_STRUCT *b) {
PmCartesian v1, v2;
PmPose p1, p2;
double dot;
if(!a || !b)
return 1;
if(a->motion_type == TC_CIRCULAR)
p1 = a->coords.circle.abc.end;
else
p1 = a->coords.line.abc.end;
if(b->motion_type == TC_CIRCULAR)
p2 = b->coords.circle.abc.start;
else
p2 = b->coords.line.abc.start;
if(!pmPosePoseCompare(p1, p2))
return 1;
v1 = tcGetEndingUnitVector(a);
v1 = tcGetEndingUnitVector(a);
v2 = tcGetStartingUnitVector(b);
pmCartCartDot(v1, v2, &dot);
return dot < 0;
}
// This is the brains of the operation. It's called every TRAJ period
// and is expected to set tp->currentPos to the new machine position.
// Lots of other tp fields (depth, done, etc) have to be twiddled to
@ -557,9 +589,8 @@ int tpRunCycle(TP_STRUCT * tp)
if(tc->maxvel > tp->vLimit)
tc->maxvel = tp->vLimit;
// honor accel constraint if we happen to make an acute angle
// with the next segment. A dot product test could often
// eliminate this.
// 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;
}
@ -567,6 +598,8 @@ int tpRunCycle(TP_STRUCT * tp)
if(nexttc && nexttc->active == 0) {
// this means this tc is being read for the first time.
TC_STRUCT *thirdtc = tcqItem(&tp->queue, 2);
nexttc->currentvel = 0;
tp->depth = tp->activeDepth = 1;
nexttc->active = 1;
@ -576,9 +609,10 @@ int tpRunCycle(TP_STRUCT * tp)
if(nexttc->maxvel > tp->vLimit)
nexttc->maxvel = tp->vLimit;
// 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)
// honor accel constraint if we happen to make an acute angle with the
// above segment or the following one, or we can't be sure
if((tc->blend_with_next && acute(tc, nexttc)) ||
(nexttc->blend_with_next && acute(nexttc, thirdtc)))
nexttc->maxaccel /= 2.0;
}
@ -829,3 +863,4 @@ int tpSetDout(TP_STRUCT *tp, int index, unsigned char start, unsigned char end)
return 0;
}
// vim:sw=4:sts=4:et: