From 38b72ae82d02131c486b80e2e8c9840c09c329ca Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 4 Mar 2015 12:07:16 -0500 Subject: [PATCH 1/2] tp: fixed bad return type for Rigid tap target and threading regression Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tc.c | 8 ++++++-- src/emc/tp/tc.h | 2 +- src/emc/tp/tp.c | 17 +++++++++-------- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index d1db49476bb..7feb524680f 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -680,10 +680,14 @@ int pmRigidTapInit(PmRigidTap * const tap, } -int pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) +double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) { // allow 10 turns of the spindle to stop - we don't want to just go on forever - return tap->xyz.tmag + 10. * uu_per_rev; + double overrun = 10. * uu_per_rev; + double target = tap->xyz.tmag + overrun; + tp_debug_print("initial tmag = %.12g, added %.12g for overrun, target = %.12g\n", + tap->xyz.tmag, overrun,target); + return target; } /** Returns true if segment has ONLY rotary motion, false otherwise. */ diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 4c496269bdc..ce0d84c75f4 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -77,7 +77,7 @@ int pmRigidTapInit(PmRigidTap * const tap, EmcPose const * const start, EmcPose const * const end); -int pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); +double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); int tcInit(TC_STRUCT * const tc, int motion_type, diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 99a3f4629af..19667a3057d 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1499,7 +1499,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { int cutoff_ratio = BLEND_DIST_FRACTION / 2.0; if (progress_ratio >= cutoff_ratio) { - tp_debug_print("segment %d has moved past %f\% progress, cannot blend safely!\n", + tp_debug_print("segment %d has moved past %f percent progress, cannot blend safely!\n", ind-1, cutoff_ratio * 100.0); return TP_ERR_OK; } @@ -2184,7 +2184,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, switch (tc->coords.rigidtap.state) { case TAPPING: - rtapi_print_msg(RTAPI_MSG_DBG, "TAPPING"); + tc_debug_print("TAPPING\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // command reversal emcmotStatus->spindle.speed *= -1.0; @@ -2192,7 +2192,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, } break; case REVERSING: - rtapi_print_msg(RTAPI_MSG_DBG, "REVERSING"); + tc_debug_print("REVERSING\n"); if (new_spindlepos < old_spindlepos) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; @@ -2211,17 +2211,17 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.state = RETRACTION; } old_spindlepos = new_spindlepos; - rtapi_print_msg(RTAPI_MSG_DBG, "Spindlepos = %f", new_spindlepos); + tc_debug_print("Spindlepos = %f\n", new_spindlepos); break; case RETRACTION: - rtapi_print_msg(RTAPI_MSG_DBG, "RETRACTION"); + tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { emcmotStatus->spindle.speed *= -1; tc->coords.rigidtap.state = FINAL_REVERSAL; } break; case FINAL_REVERSAL: - rtapi_print_msg(RTAPI_MSG_DBG, "FINAL_REVERSAL"); + tc_debug_print("FINAL_REVERSAL\n"); if (new_spindlepos > old_spindlepos) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; @@ -2239,7 +2239,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, old_spindlepos = new_spindlepos; break; case FINAL_PLACEMENT: - rtapi_print_msg(RTAPI_MSG_DBG, "FINAL_PLACEMENT\n"); + tc_debug_print("FINAL_PLACEMENT\n"); // this is a regular move now, it'll stop at target above. break; } @@ -2525,7 +2525,8 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { if (segment_time < cutoff_time && tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE && - tc->term_cond == TC_TERM_COND_TANGENT) + tc->term_cond == TC_TERM_COND_TANGENT && + tc->motion_type != TC_RIGIDTAP) { tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n", segment_time, cutoff_time); From 99e9c6dec0a52af87cf8ebe606ed8712f73e4724 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 4 Mar 2015 12:08:28 -0500 Subject: [PATCH 2/2] posemath: fix for argument types when in debug mode due to pass-by-reference change introduced with new TP Signed-off-by: Robert W. Ellenberg --- src/libnml/posemath/_posemath.c | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 20000b2def5..9c238dd7548 100644 --- a/src/libnml/posemath/_posemath.c +++ b/src/libnml/posemath/_posemath.c @@ -221,7 +221,9 @@ int pmRotQuatConvert(PmRotationVector const * const r, PmQuaternion * const q) #ifdef PM_DEBUG /* make sure r is normalized */ - if (0 != pmRotNorm(r, &r)) { + //FIXME breaks const promise + PmRotationVector r_raw = *r; + if (0 != pmRotNorm(&r_raw, r)) { #ifdef PM_PRINT_ERROR pmPrintError ("error: pmRotQuatConvert rotation vector not normalized\n"); @@ -325,7 +327,7 @@ int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r) double sh; #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q)) { + if (!pmQuatIsNorm(q)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmQuatRotConvert\n"); #endif @@ -356,7 +358,7 @@ int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r) int pmQuatMatConvert(PmQuaternion const * const q, PmRotationMatrix * const m) { #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q)) { + if (!pmQuatIsNorm(q)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmQuatMatConvert\n"); #endif @@ -1070,7 +1072,7 @@ int pmQuatAxisAngleMult(PmQuaternion const * const q, PmAxis axis, double angle, double sh, ch; #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q)) { + if (!pmQuatIsNorm(q)) { #ifdef PM_PRINT_ERROR pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n"); #endif @@ -1275,7 +1277,7 @@ int pmMatMatMult(PmRotationMatrix const * const m1, PmRotationMatrix const * con int pmQuatQuatCompare(PmQuaternion const * const q1, PmQuaternion const * const q2) { #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q1) || !pmQuatIsNorm(&q2)) { + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmQuatQuatCompare\n"); #endif @@ -1358,7 +1360,7 @@ int pmQuatInv(PmQuaternion const * const q1, PmQuaternion * const qout) qout->z = -q1->z; #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q1)) { + if (!pmQuatIsNorm(q1)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmQuatInv\n"); #endif @@ -1421,7 +1423,7 @@ int pmQuatQuatMult(PmQuaternion const * const q1, PmQuaternion const * const q2, } #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q1) || !pmQuatIsNorm(&q2)) { + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmQuatQuatMult\n"); #endif @@ -1445,7 +1447,7 @@ int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, vout->z = v2->z + 2.0 * (q1->s * c.z + q1->x * c.y - q1->y * c.x); #ifdef PM_DEBUG - if (!pmQuatIsNorm(&q1)) { + if (!pmQuatIsNorm(q1)) { #ifdef PM_PRINT_ERROR pmPrintError(&"Bad quaternion in pmQuatCartMult\n"); #endif @@ -1476,7 +1478,7 @@ int pmPoseInv(PmPose const * const p1, PmPose * const p2) int r1, r2; #ifdef PM_DEBUG - if (!pmQuatIsNorm(&p1.rot)) { + if (!pmQuatIsNorm(&p1->rot)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad quaternion in pmPoseInv\n"); #endif @@ -1539,7 +1541,7 @@ int pmHomInv(PmHomogeneous const * const h1, PmHomogeneous * const h2) int r1, r2; #ifdef PM_DEBUG - if (!pmMatIsNorm(h1->rot)) { + if (!pmMatIsNorm(&h1->rot)) { #ifdef PM_PRINT_ERROR pmPrintError("Bad rotation matrix in pmHomInv\n"); #endif