docs: Update Kinematics.md document with "smooth pressure advance"

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 145 insertions, 0 deletions

diff --git a/scripts/graph_extruder.py b/scripts/graph_extruder.py
new file mode 100755
index 00000000..6223bb11
--- /dev/null
+++ b/scripts/graph_extruder.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python2
+# Generate extruder pressure advance motion graphs
+#
+# Copyright (C) 2019  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, optparse, datetime
+import matplotlib
+
+SEG_TIME = .000100
+INV_SEG_TIME = 1. / SEG_TIME
+
+
+######################################################################
+# Basic trapezoid motion
+######################################################################
+
+# List of moves: [(start_v, end_v, move_t), ...]
+Moves = [
+    (0., 0., .200),
+    (0., 100., None), (100., 100., .200), (100., 60., None),
+    (60., 100., None), (100., 100., .200), (100., 0., None),
+    (0., 0., .300)
+]
+EXTRUDE_R = (.4 * .4 * .75) / (math.pi * (1.75 / 2.)**2)
+ACCEL = 3000. * EXTRUDE_R
+
+def gen_positions():
+    out = []
+    start_d = start_t = t = 0.
+    for start_v, end_v, move_t in Moves:
+        start_v *= EXTRUDE_R
+        end_v *= EXTRUDE_R
+        if move_t is None:
+            move_t = abs(end_v - start_v) / ACCEL
+        half_accel = 0.
+        if end_v > start_v:
+            half_accel = .5 * ACCEL
+        elif start_v > end_v:
+            half_accel = -.5 * ACCEL
+        end_t = start_t + move_t
+        while t <= end_t:
+            rel_t = t - start_t
+            out.append(start_d + (start_v + half_accel * rel_t) * rel_t)
+            t += SEG_TIME
+        start_d += (start_v + half_accel * move_t) * move_t
+        start_t = end_t
+    return out
+
+def gen_deriv(data):
+    return [0.] + [(data[i+1] - data[i]) * INV_SEG_TIME
+                   for i in range(len(data)-1)]
+
+def time_to_index(t):
+    return int(t * INV_SEG_TIME + .5)
+
+
+######################################################################
+# Pressure advance
+######################################################################
+
+PA_HALF_SMOOTH_T = .040 / 2.
+PRESSURE_ADVANCE = .045
+
+def calc_pa_raw(t, positions):
+    pa = PRESSURE_ADVANCE * INV_SEG_TIME
+    i = time_to_index(t)
+    return positions[i] + pa * (positions[i+1] - positions[i])
+
+def calc_pa_smooth(t, positions):
+    start_index = time_to_index(t - PA_HALF_SMOOTH_T) + 1
+    end_index = time_to_index(t + PA_HALF_SMOOTH_T)
+    pa = PRESSURE_ADVANCE * INV_SEG_TIME
+    pa_data = [positions[i] + pa * (positions[i+1] - positions[i])
+               for i in range(start_index, end_index)]
+    return sum(pa_data) / (end_index - start_index)
+
+
+######################################################################
+# Plotting and startup
+######################################################################
+
+MARGIN_TIME = 0.100
+
+def plot_motion():
+    # Nominal motion
+    positions = gen_positions()
+    drop = int(MARGIN_TIME * INV_SEG_TIME)
+    times = [SEG_TIME * t for t in range(len(positions))][drop:-drop]
+    velocities = gen_deriv(positions[drop:-drop])
+    # Motion with pressure advance
+    pa_positions = [calc_pa_raw(t, positions) for t in times]
+    pa_velocities = gen_deriv(pa_positions)
+    # Smoothed motion
+    sm_positions = [calc_pa_smooth(t, positions) for t in times]
+    sm_velocities = gen_deriv(sm_positions)
+    # Build plot
+    shift_times = [t - MARGIN_TIME for t in times]
+    fig, ax1 = matplotlib.pyplot.subplots(nrows=1, sharex=True)
+    ax1.set_title("Extruder Velocity")
+    ax1.set_ylabel('Velocity (mm/s)')
+    pa_plot, = ax1.plot(shift_times, pa_velocities, 'r',
+                        label='Pressure Advance', alpha=0.3)
+    nom_plot, = ax1.plot(shift_times, velocities, 'black', label='Nominal')
+    sm_plot, = ax1.plot(shift_times, sm_velocities, 'g', label='Smooth PA',
+                        alpha=0.9)
+    fontP = matplotlib.font_manager.FontProperties()
+    fontP.set_size('x-small')
+    ax1.legend(handles=[nom_plot, pa_plot, sm_plot], loc='best', prop=fontP)
+    ax1.set_xlabel('Time (s)')
+    ax1.grid(True)
+    fig.tight_layout()
+    return fig
+
+def setup_matplotlib(output_to_file):
+    global matplotlib
+    if output_to_file:
+        matplotlib.rcParams.update({'figure.autolayout': True})
+        matplotlib.use('Agg')
+    import matplotlib.pyplot, matplotlib.dates, matplotlib.font_manager
+    import matplotlib.ticker
+
+def main():
+    # Parse command-line arguments
+    usage = "%prog [options]"
+    opts = optparse.OptionParser(usage)
+    opts.add_option("-o", "--output", type="string", dest="output",
+                    default=None, help="filename of output graph")
+    options, args = opts.parse_args()
+    if len(args) != 0:
+        opts.error("Incorrect number of arguments")
+
+    # Draw graph
+    setup_matplotlib(options.output is not None)
+    fig = plot_motion()
+
+    # Show graph
+    if options.output is None:
+        matplotlib.pyplot.show()
+    else:
+        fig.set_size_inches(6, 2.5)
+        fig.savefig(options.output)
+
+if __name__ == '__main__':
+    main()