summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorRobert W. Ellenberg <rwe24g@gmail.com>2014-01-27 19:37:40 -0500
committerChris Radek <chris@timeguy.com>2014-06-11 14:44:03 -0500
commit38589349a2897a55e67433331c3f00764919d890 (patch)
treeb3f0e03aa5da53686a492d4eca262c568919031d
parent6d4abbeb41af5cf66f0f53657c99e3437fa75325 (diff)
downloadlinuxcnc-38589349a2897a55e67433331c3f00764919d890.tar.gz
linuxcnc-38589349a2897a55e67433331c3f00764919d890.zip
Added Gap cycles as replacement for smoothing setting
Diffstat
-rw-r--r--src/emc/ini/initraj.cc6
-rw-r--r--src/emc/motion/command.c2
-rw-r--r--src/emc/motion/motion.h4
-rw-r--r--src/emc/nml_intf/emc.hh2
-rw-r--r--src/emc/task/taskintf.cc4
-rw-r--r--src/emc/tp/tp.c35
-rw-r--r--src/emc/tp/tp.h2
-rw-r--r--tests/trajectory-planner/circular-arcs/circular_arcs.ini6
-rw-r--r--tests/trajectory-planner/circular-arcs/circular_arcs_rt.ini7
-rwxr-xr-xtests/trajectory-planner/circular-arcs/test-debug.sh5
10 files changed, 38 insertions, 35 deletions
diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc
index 7d96fd932..8dd8a732f 100644
--- a/src/emc/ini/initraj.cc
+++ b/src/emc/ini/initraj.cc
@@ -166,17 +166,17 @@ static int loadTraj(EmcIniFile *trajInifile)
int arcBlendEnable = 0;
int arcBlendFallbackEnable = 0;
int arcBlendOptDepth = 0;
- double arcBlendSmoothingThreshold = 0.4;
+ double arcBlendGapCycles = 2;
double arcBlendGreediness = 0.5;
trajInifile->Find(&arcBlendEnable, "ARC_BLEND_ENABLE", "TRAJ");
trajInifile->Find(&arcBlendFallbackEnable, "ARC_BLEND_FALLBACK_ENABLE", "TRAJ");
trajInifile->Find(&arcBlendOptDepth, "ARC_BLEND_OPTIMIZATION_DEPTH", "TRAJ");
- trajInifile->Find(&arcBlendSmoothingThreshold, "ARC_BLEND_SMOOTHING_THRESHOLD", "TRAJ");
+ trajInifile->Find(&arcBlendGapCycles, "ARC_BLEND_GAP_CYCLES", "TRAJ");
trajInifile->Find(&arcBlendGreediness, "ARC_BLEND_GREEDINESS", "TRAJ");
if (0 != emcSetupArcBlends(arcBlendEnable, arcBlendFallbackEnable,
- arcBlendOptDepth, arcBlendSmoothingThreshold, arcBlendGreediness)) {
+ arcBlendOptDepth, arcBlendGapCycles, arcBlendGreediness)) {
if (emc_debug & EMC_DEBUG_CONFIG) {
rcs_print("bad return value from emcSetupArcBlends\n");
}
diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c
index 85af85f93..edb50a926 100644
--- a/src/emc/motion/command.c
+++ b/src/emc/motion/command.c
@@ -1645,7 +1645,7 @@ check_stuff ( "before command_handler()" );
emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable;
emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable;
emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth;
- emcmotConfig->arcBlendSmoothingThreshold = emcmotCommand->arcBlendSmoothingThreshold;
+ emcmotConfig->arcBlendGapCycles = emcmotCommand->arcBlendGapCycles;
emcmotConfig->arcBlendGreediness = emcmotCommand->arcBlendGreediness;
break;
diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h
index f2b4c4df2..1babb74ba 100644
--- a/src/emc/motion/motion.h
+++ b/src/emc/motion/motion.h
@@ -252,7 +252,7 @@ extern "C" {
int arcBlendOptDepth;
int arcBlendEnable;
int arcBlendFallbackEnable;
- double arcBlendSmoothingThreshold;
+ double arcBlendGapCycles;
double arcBlendGreediness;
double maxFeedScale;
} emcmot_command_t;
@@ -745,7 +745,7 @@ Suggestion: Split this in to an Error and a Status flag register..
int arcBlendOptDepth;
int arcBlendEnable;
int arcBlendFallbackEnable;
- double arcBlendSmoothingThreshold;
+ double arcBlendGapCycles;
double arcBlendGreediness;
double maxFeedScale;
} emcmot_config_t;
diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh
index fa0a1b654..2e1f3e084 100644
--- a/src/emc/nml_intf/emc.hh
+++ b/src/emc/nml_intf/emc.hh
@@ -550,7 +550,7 @@ int emcSetMaxFeedOverride(double maxFeedScale);
int emcSetupArcBlends(int arcBlendEnable,
int arcBlendFallbackEnable,
int arcBlendOptDepth,
- double arcBlendSmoothingThreshold,
+ double arcBlendGapCycles,
double arcBlendGreediness);
extern int emcUpdate(EMC_STAT * stat);
diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc
index dfc94a83f..d9f19f33c 100644
--- a/src/emc/task/taskintf.cc
+++ b/src/emc/task/taskintf.cc
@@ -1557,14 +1557,14 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat)
int emcSetupArcBlends(int arcBlendEnable,
int arcBlendFallbackEnable,
int arcBlendOptDepth,
- double arcBlendSmoothingThreshold,
+ double arcBlendGapCycles,
double arcBlendGreediness) {
emcmotCommand.command = EMCMOT_SETUP_ARC_BLENDS;
emcmotCommand.arcBlendEnable = arcBlendEnable;
emcmotCommand.arcBlendFallbackEnable = arcBlendFallbackEnable;
emcmotCommand.arcBlendOptDepth = arcBlendOptDepth;
- emcmotCommand.arcBlendSmoothingThreshold = arcBlendSmoothingThreshold;
+ emcmotCommand.arcBlendGapCycles = arcBlendGapCycles;
emcmotCommand.arcBlendGreediness = arcBlendGreediness;
return usrmotWriteEmcmotCommand(&emcmotCommand);
}
diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c
index cdef2cb70..d26edd9c2 100644
--- a/src/emc/tp/tp.c
+++ b/src/emc/tp/tp.c
@@ -60,6 +60,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp,
TC_STRUCT * const tc);
STATIC int tpRunOptimization(TP_STRUCT * const tp);
+STATIC double saturate(double x, double max);
//Empty function to act as an assert for GDB in simulation
int gdb_fake_catch(int condition){
@@ -886,8 +887,8 @@ STATIC int tpInitBlendArc(TP_STRUCT const * const tp, TC_STRUCT const * const pr
}
-STATIC double tcFindBlendTolerance(TC_STRUCT const * const prev_tc,
- TC_STRUCT const * const tc)
+STATIC int tcFindBlendTolerance(TC_STRUCT const * const prev_tc,
+ TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance)
{
const double tolerance_ratio = 0.25;
double T1 = fmin(prev_tc->tolerance, prev_tc->nominal_length * tolerance_ratio);
@@ -899,8 +900,14 @@ STATIC double tcFindBlendTolerance(TC_STRUCT const * const prev_tc,
if (T2 == 0) {
T2 = tc->nominal_length * tolerance_ratio;
}
- double tolerance = fmin(T1,T2);
- return tolerance;
+ *nominal_tolerance = fmin(T1,T2);
+ //Blend tolerance is the limit of what we can reach by blending alone,
+ //consuming half a segment or less (parabolic equivalent)
+ double blend_tolerance = fmin(fmin(*nominal_tolerance,
+ prev_tc->nominal_length * tolerance_ratio),
+ tc->nominal_length * tolerance_ratio);
+ *T_blend = blend_tolerance;
+ return TP_ERR_OK;
}
/**
@@ -952,15 +959,15 @@ STATIC int tpCreateBlendArc(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
double phi = (PM_PI - theta * 2.0);
- PmCartesian normal;
+ PmCartesian binormal;
pmCartCartCross(&prev_tc->coords.line.xyz.uVec,
&tc->coords.line.xyz.uVec,
- &normal);
- pmCartUnit(&normal,&normal);
- tp_debug_print("normal = [%f %f %f]\n",normal.x, normal.y,normal.z);
+ &binormal);
+ pmCartUnit(&binormal,&binormal);
+ tp_debug_print("binormal = [%f %f %f]\n", binormal.x,binormal.y,binormal.z);
double a_max;
//TODO move this function into setup somewhere because this should be constant
- tpGetPlanarAccelLimit(&normal, &a_max);
+ tpGetPlanarAccelLimit(&binormal, &a_max);
double a_n_max = a_max * TP_ACC_RATIO_NORMAL;
tp_debug_print("a_max = %f, a_n_max = %f\n",a_max, a_n_max);
@@ -983,9 +990,9 @@ STATIC int tpCreateBlendArc(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
double min_segment_time = tp->cycleTime * TP_MIN_SEGMENT_CYCLES;
- //Find blend tolerance from prev and current lines
- //Find the minimum tolerance (in case it dropped between moves)
- double tolerance = tcFindBlendTolerance(prev_tc, tc);
+ //TODO get tolerance from blend here
+ double tolerance,nominal_tolerance;
+ tcFindBlendTolerance(prev_tc,tc,&tolerance,&nominal_tolerance);
double h_tol = tolerance / (1.0 - Stheta);
double d_tol = Ctheta * h_tol;
// Debug output for tolerances
@@ -1034,7 +1041,7 @@ STATIC int tpCreateBlendArc(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
double L_next = tc->target - d_plan;
#endif
- int consume = (prev_seg_time < min_segment_time);
+ int consume = (prev_seg_time < emcmotConfig->arcBlendGapCycles * min_segment_time);
double s_arc = phi * R_plan;
if (consume) {
s_arc += L_prev;
@@ -1106,6 +1113,7 @@ STATIC int tpCreateBlendArc(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
if (consume) {
prev_tc->smoothing = 1;
blend_tc->smoothing = 1;
+
retval = tcqPopBack(&tp->queue);
tp_debug_print("consume previous line\n");
if (retval) {
@@ -1118,6 +1126,7 @@ STATIC int tpCreateBlendArc(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
blend_tc->coords.arc.xyz.line_length = 0;
retval = tcConnectBlendArc(prev_tc, tc, &circ_start, &circ_end);
}
+ tpFinalizeSegmentLimits(tp, blend_tc);
return retval;
diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h
index 9fd82007e..bddc7b578 100644
--- a/src/emc/tp/tp.h
+++ b/src/emc/tp/tp.h
@@ -22,8 +22,8 @@
#define TP_MIN_SEGMENT_CYCLES 1.02
//Values chosen for accel ratio to match parabolic blend acceleration
//limits.
-#define TP_ACC_RATIO_NORMAL (pmSqrt(3.0)/2.0)
#define TP_ACC_RATIO_TANGENTIAL 0.5
+#define TP_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(TP_ACC_RATIO_TANGENTIAL)))
#define TP_OPTIMIZATION_CUTOFF 4
/* closeness to zero, for determining if a move is pure rotation */
diff --git a/tests/trajectory-planner/circular-arcs/circular_arcs.ini b/tests/trajectory-planner/circular-arcs/circular_arcs.ini
index c4bfc7b95..97f445313 100644
--- a/tests/trajectory-planner/circular-arcs/circular_arcs.ini
+++ b/tests/trajectory-planner/circular-arcs/circular_arcs.ini
@@ -129,10 +129,8 @@ MAX_LINEAR_VELOCITY = 12
ARC_BLEND_ENABLE = 1
ARC_BLEND_FALLBACK_ENABLE = 0
ARC_BLEND_OPTIMIZATION_DEPTH = 50
-#Assume smooth velocity changes for line segments that are reduced in length by this factor
-ARC_BLEND_SMOOTHING_THRESHOLD = .80
-#How much (0.0 .. 1.0) of the next segment a blend arc is allowed to consume
-ARC_BLEND_GREEDINESS = 0.65
+ARC_BLEND_GAP_CYCLES = 2
+ARC_BLEND_GREEDINESS = 0.5
# Axes sections ---------------------------------------------------------------
diff --git a/tests/trajectory-planner/circular-arcs/circular_arcs_rt.ini b/tests/trajectory-planner/circular-arcs/circular_arcs_rt.ini
index aa3058738..b4bc0f053 100644
--- a/tests/trajectory-planner/circular-arcs/circular_arcs_rt.ini
+++ b/tests/trajectory-planner/circular-arcs/circular_arcs_rt.ini
@@ -128,11 +128,8 @@ MAX_LINEAR_VELOCITY = 12
ARC_BLEND_ENABLE = 1
ARC_BLEND_FALLBACK_ENABLE = 0
ARC_BLEND_OPTIMIZATION_DEPTH = 50
-#Use this setting for no smoothing (for debugging and stress-testing)
-ARC_BLEND_SMOOTHING_THRESHOLD = .3
-#Use this setting for "normal" smoothing, i.e. if we blend over more than 40% of a segment
-#ARC_BLEND_SMOOTHING_THRESHOLD = 0.40
-ARC_BLEND_GREEDINESS = 0.6
+ARC_BLEND_GAP_CYCLES = 2
+ARC_BLEND_GREEDINESS = 0.5
# Axes sections ---------------------------------------------------------------
diff --git a/tests/trajectory-planner/circular-arcs/test-debug.sh b/tests/trajectory-planner/circular-arcs/test-debug.sh
index e94f52c90..96013a8c0 100755
--- a/tests/trajectory-planner/circular-arcs/test-debug.sh
+++ b/tests/trajectory-planner/circular-arcs/test-debug.sh
@@ -2,12 +2,11 @@
set -o monitor
./build-debug.sh
cp position.blank position.txt
-linuxcnc configs/circular_arcs_medium.ini > test.log &
-python machine_setup.py $1
+linuxcnc $1 > test.log &
+python machine_setup.py $2
fg
./process_runlog.sh test.log
#if [ -a movement.log ]
#then
#octave/plot_movement.m
#fi
-exit $1
generated by cgit v1.2.3 (git 2.39.1) at 2025-05-07 08:53:32 +0000