thequux/film-scanner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added section on polynomial evaluation to the notebook

Browse Source
This commit is contained in:
TQ Hirsch
2022-04-14 14:43:33 +02:00
parent 5c880222fd
commit b066e4ceaf
3 changed files with 73 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
docs/.envrc Normal file
View File

@@ -0,0 +1 @@
eval "$(lorri direnv)"

605
docs/motion-control.ipynb Normal file
View File

@@ -0,0 +1,605 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"id": "10e2e893-05e2-4d0d-8a31-454372ef4c65",
"metadata": {},
"outputs": [],
"source": [
"import sympy\n",
"sympy.init_printing()"
]
},
{
"cell_type": "markdown",
"id": "97f366f8-d3c5-487d-b38b-5db31804a604",
"metadata": {},
"source": [
"We attempt to model motion as a series of piecewise-constant jerk segments (where jerk is $d^4p \\over dp^4$)\n",
"\n",
"For an individual segment, we have:"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "76a230d3-e50b-450e-bd40-47d7a9632043",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"sympy.core.symbol.Symbol"
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"sympy.var(\"p,v,a,j,t,v0,a0, a_max, v_max, j_max, t_a, t_v, t_j\", real=True, positive=True)\n",
"sympy.Symbol"
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "475d7b0c-7961-4554-b1ef-acea504d2010",
"metadata": {},
"outputs": [],
"source": [
"def seg_d(j=j, a0=a0, v0=v0, t=t, p0=0, following=None):\n",
" if following is not None:\n",
" a0 = following[\"ae\"]\n",
" v0 = following[\"ve\"]\n",
" p0 = following[\"pe\"]\n",
" res = dict(\n",
" dt = t,\n",
" da = t * j,\n",
" dv = j * t**2 / 2 + a0*t,\n",
" dp = j*t**3/6 + a0 * t**2 / 2 + v0 * t\n",
" )\n",
" res[\"ae\"] = res[\"da\"] + a0\n",
" res[\"ve\"] = res[\"dv\"] + v0\n",
" res[\"pe\"] = p0 + res[\"dp\"]\n",
" return dict((k, v.simplify()) for k,v in res.items())\n",
"seg_p = seg_d()[\"dp\"]"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "378b1d65-afdb-496a-8217-f17ce9e92783",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle p = \\frac{t \\left(3 a_{0} t + j t^{2} + 6 v_{0}\\right)}{6}$"
],
"text/plain": [
" ⎛ 2 ⎞\n",
" t⋅⎝3⋅a₀⋅t + j⋅t + 6⋅v₀⎠\n",
"p = ────────────────────────\n",
" 6 "
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"sympy.Eq(p, seg_p)"
]
},
{
"cell_type": "markdown",
"id": "6874bd52-7f23-494b-bf5a-5d01f141cae6",
"metadata": {},
"source": [
"We have three machine limits to consider at this point: we have a maximum jerk (derived from the shock limits of the system), maximum acceleration (derived from the torque limits of the servos), and maximum velocity (from the frequency at which we update the steppers). We can ignore the position bounds at this point; the commands to this stage of the motion planner are simply \"move by ΔX,ΔY\", and so we can assume that the source of those commands will never ask us to move outside the range of motion.\n",
"\n",
"This leaves us with the following general motion profile for a single linear move:\n",
"1. $j>0$; Increase acceleration to $a_{max}$\n",
"2. $j=0$; continue accelerating at $a_{max}$ to somewhat before $v_{max}$\n",
"3. $j<0$; reduce acceleration to 0 to reach constant velocity $v_{max}$\n",
"4. $j=0$; proceed at constant velocity to decelleration point\n",
"5. $j<0$; begin decelleration to $-a_{max}$\n",
"6. $j=0$; decelerate at max rate to just before 0 velocity\n",
"7. $j>0$; decrease decelleration until $v=0$\n",
"\n",
"Let's call the time taken for segment $n$ \"$t_n$\". Note that, through a simple coordinate transform, $t_1=t_3=t_5=t_7$. Further, $t_2=t_6$. We'll rename these to $t_j$ and $t_a$ (for constant jerk and constant accelleration), respectively, and introduce $t_v = t_4$ for consistency.\n",
"\n",
"\n",
"There are several possible degenerate cases:\n",
"\n",
"* The move is too short to reach $v_{max}$. In this case, we lower $v_max$ accordingly and set $t_v=0$.\n",
"* $a_{max}$ is unreachable given the maximum velocity. This results in $t_a$ both being 0 for all moves and the calculation proceeding with lowered $a_{max}$\n",
"\n",
"Note that the first situation can give rise to the second.\n",
"\n",
"We can handle these degenerate situations using the following algorithm:\n",
"\n",
"1. Assume the most general form of the profile. In this case, all but stage 4 is constant-time, so solve for $t_v$\n",
"2. If the calculated $t_v < 0$, calculate the actual $v_{max}$ and recalculate $t_a$ holding $t_j$ constant and $t_v=0$.\n",
"3. If $t_a<0$, recalculate $t_j$ holding $t_v=t_a=0$\n",
"\n",
"This means that to solve the motion equations, we need a couple of closed-form solutions:"
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "a1722a31-0b31-4cfd-8179-4222fdb2fb56",
"metadata": {},
"outputs": [],
"source": [
"# Time and position calculations for general form\n",
"t_j_max = a_max / j_max\n",
"p1s1 = seg_d(j=j_max, a0=0, v0=0, t=t_j)\n",
"# p1s1"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "9554e8a3-f968-4ae2-b0b0-52e0e6489631",
"metadata": {},
"outputs": [],
"source": [
"p1s2 = seg_d(j = 0, t = t_a, following=p1s1)\n",
"# p1s2"
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "56e480b7-0f00-4791-a443-b4891bb522b1",
"metadata": {},
"outputs": [],
"source": [
"p1s3 = seg_d(j=-j_max, t=t_j, following=p1s2)\n",
"# p1s3"
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "77436da4-316f-4b05-b4fa-431468b1ba16",
"metadata": {},
"outputs": [],
"source": [
"t_a_max = sympy.solve(p1s3[\"ve\"].subs(t_j, t_j_max) - v_max, t_a)[0]"
]
},
{
"cell_type": "code",
"execution_count": 9,
"id": "6494ee63-9e1f-4482-864a-4f14683f3fb0",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\frac{j_{max} t_{j} \\left(t_{a}^{2} + 3 t_{a} t_{j} + 2 t_{j}^{2}\\right)}{2}$"
],
"text/plain": [
" ⎛ 2 2⎞\n",
"jₘₐₓ⋅t_j⋅⎝tₐ + 3⋅tₐ⋅t_j + 2⋅t_j ⎠\n",
"──────────────────────────────────\n",
" 2 "
]
},
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s3[\"pe\"]"
]
},
{
"cell_type": "code",
"execution_count": 10,
"id": "7add3da9-fe63-40a8-973a-75d1214fd8db",
"metadata": {},
"outputs": [],
"source": [
"p1s4 = seg_d(j=0, t=t_v, following=p1s3)"
]
},
{
"cell_type": "code",
"execution_count": 11,
"id": "b81e62d7-62ec-4d11-8ba4-c9c8d460ebfb",
"metadata": {},
"outputs": [],
"source": [
"p1s5 = seg_d(j=-j_max, t=t_j, following=p1s4)\n",
"p1s6 = seg_d(j=0, t=t_a, following=p1s5)\n",
"p1s7 = seg_d(j=j_max, t=t_j, following=p1s6)"
]
},
{
"cell_type": "code",
"execution_count": 12,
"id": "46c5af02-fc38-4630-a3ab-11275d5c0c0e",
"metadata": {
"tags": []
},
"outputs": [
{
"data": {
"text/plain": [
"{'dt': t_v,\n",
" 'da': 0,\n",
" 'dv': 0,\n",
" 'dp': j_max*t_j*t_v*(t_a + t_j),\n",
" 'ae': 0,\n",
" 've': j_max*t_j*(t_a + t_j),\n",
" 'pe': j_max*t_j*(t_a**2 + 3*t_a*t_j + 2*t_j**2 + 2*t_v*(t_a + t_j))/2}"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s4"
]
},
{
"cell_type": "code",
"execution_count": 13,
"id": "f2ae9840-53ff-4295-8555-fc5ae26affcf",
"metadata": {
"tags": []
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle 0$"
],
"text/plain": [
"0"
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"(p1s3[\"pe\"] - sum(x[\"dp\"] for x in [p1s5, p1s6, p1s7])).subs({t_j: t_j_max, t_a: t_a_max}).simplify()"
]
},
{
"cell_type": "code",
"execution_count": 14,
"id": "ceeac2db-ca02-4a7f-a1cd-5b48c9b944ca",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\frac{a_{max} v_{max}}{j_{max}} + t_{v} v_{max} + \\frac{v_{max}^{2}}{a_{max}}$"
],
"text/plain": [
" 2\n",
"aₘₐₓ⋅vₘₐₓ vₘₐₓ \n",
"───────── + tᵥ⋅vₘₐₓ + ─────\n",
" jₘₐₓ aₘₐₓ"
]
},
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s7[\"pe\"].subs({t_j: t_j_max, t_a: t_a_max}).simplify()"
]
},
{
"cell_type": "code",
"execution_count": 15,
"id": "35f7b0c4-da31-4fda-9feb-987fcc43626a",
"metadata": {},
"outputs": [],
"source": [
"p1 = p1s7[\"pe\"].subs({t_j: t_j_max, t_a: t_a_max}).simplify()\n",
"p2 = p1s7[\"pe\"].subs({t_j: t_j_max, t_v: 0}).simplify()\n",
"p3 = p1s7[\"pe\"].subs({t_a: 0, t_v: 0}).simplify()"
]
},
{
"cell_type": "code",
"execution_count": 16,
"id": "ebd50b37-b67e-4861-ae03-ccb901886457",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle t_{v} = - \\frac{a_{max}}{j_{max}} + \\frac{p}{v_{max}} - \\frac{v_{max}}{a_{max}}$"
],
"text/plain": [
" aₘₐₓ p vₘₐₓ\n",
"tᵥ = - ──── + ──── - ────\n",
" jₘₐₓ vₘₐₓ aₘₐₓ"
]
},
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1tv = sympy.solve(p - p1, t_v)[0]\n",
"sympy.Eq(t_v, p1tv)"
]
},
{
"cell_type": "code",
"execution_count": 17,
"id": "5f779934-7f89-4ef4-9f89-d268a9742059",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle t_{a} = \\frac{- 3 a_{max}^{\\frac{3}{2}} + \\sqrt{a_{max}^{3} + 4 j_{max}^{2} p}}{2 \\sqrt{a_{max}} j_{max}}$"
],
"text/plain": [
" ___________________\n",
" 3/2 ╱ 3 2 \n",
" - 3⋅aₘₐₓ + ╲╱ aₘₐₓ + 4⋅jₘₐₓ ⋅p \n",
"tₐ = ────────────────────────────────────\n",
" ______ \n",
" 2⋅╲╱ aₘₐₓ ⋅jₘₐₓ "
]
},
"execution_count": 17,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p2ta = sympy.solve(p - p2, t_a, positive=True)[0]\n",
"sympy.Eq(t_a, p2ta)"
]
},
{
"cell_type": "code",
"execution_count": 18,
"id": "5e61c9d2-09f5-4585-999a-e9c786451520",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle t_{j} = \\frac{2^{\\frac{2}{3}} \\sqrt[3]{p}}{2 \\sqrt[3]{j_{max}}}$"
],
"text/plain": [
" 2/3 3 ___\n",
" 2 ⋅╲╱ p \n",
"t_j = ──────────\n",
" 3 ______\n",
" 2⋅╲╱ jₘₐₓ "
]
},
"execution_count": 18,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p3tj = sympy.solve(p - p3, t_j)[0]\n",
"sympy.Eq(t_j, p3tj)"
]
},
{
"cell_type": "markdown",
"id": "232bf430-e752-48a2-8fd7-420e330b1be1",
"metadata": {},
"source": [
"# Timing calculations\n",
"\n",
"Now that we have closed-form solutions for each of the timing regimes, we need to know when those formulas apply.\n",
"\n",
"We do this by evaluating $p_{max}$ at each of the transition points:"
]
},
{
"cell_type": "code",
"execution_count": 19,
"id": "6e63f6bc-a333-4b18-b52f-9519388bb610",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[ \\frac{2 a_{max}^{3}}{j_{max}^{2}}, \\ \\frac{a_{max} v_{max}}{j_{max}} + \\frac{v_{max}^{2}}{a_{max}}\\right]$"
],
"text/plain": [
"⎡ 3 2⎤\n",
"⎢2⋅aₘₐₓ aₘₐₓ⋅vₘₐₓ vₘₐₓ ⎥\n",
"⎢───────, ───────── + ─────⎥\n",
"⎢ 2 jₘₐₓ aₘₐₓ⎥\n",
"⎣ jₘₐₓ ⎦"
]
},
"execution_count": 19,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"[ p1s7[\"pe\"].subs(param).simplify() for param in [{t_v: 0, t_a: 0, t_j: t_j_max}, {t_v: 0, t_a: t_a_max, t_j: t_j_max}]]"
]
},
{
"cell_type": "markdown",
"id": "5f6fac1d-4a2b-4045-ad31-0f6b26d3f6dd",
"metadata": {},
"source": [
"The first of these is where we transition from adjusting $t_j$ to $t_a$, and the second is where we start manipulating $t_v$."
]
},
{
"cell_type": "markdown",
"id": "0d85e9c4-f311-4dfe-8762-30b9e4f95b42",
"metadata": {},
"source": [
"# Control loop simulation\n",
"\n",
"All of these assume that the control loop runs with infinite precision, which is patently false. Rather, for stability, we want to use fixed-point arithmetic as much as possible, using steps/ticks as our units.\n",
"\n",
"To evaluate our results, we implement a simple motion planner and stepper driver simulation, assuming that the driven stage is purely cartesian. When we move to the CoreXY simulation, we will need to perform the coordinate system rotation before motion planning to make this much easier to manage. Alternatively, we could measure position in *half*-steps and decide which side to vibrate on, but TBH, working in motor coordinates seems easier."
]
},
{
"cell_type": "code",
"execution_count": 20,
"id": "dab40dd2-2b62-4445-9a7a-4b02ae87fef0",
"metadata": {},
"outputs": [],
"source": [
"class StepperSimulator:\n",
" def __init__(self):\n",
" self.data = []\n",
" self.pos = 0\n",
" self.t = 0\n",
" self.last_step = False\n",
" def tick(self, step, dir):\n",
" self.t += 1\n",
" if step and not self.last_step:\n",
" self.pos += 1 if dir else -1\n",
" self.data.push((self.t, self.pos))\n",
" self.last_step = step\n",
"\n",
"class MotionPlanner:\n",
" MP_FRAC = 8 # 8 fractional bits\n",
" MP_TICK_PER_SEC = 5000\n",
" MP_MM_PER_STEP = 0.000\n",
" MP_AMAX = 1\n",
" "
]
},
{
"cell_type": "markdown",
"id": "7286a7a2-f716-4f5a-b736-0eb97b90c6f1",
"metadata": {},
"source": [
"# Implementation notes\n",
"\n",
"## Units\n",
"If we choose our units judiciously, all calculations can be integers, removing the need for FP computations in the critical loop and thus removing floating-point inaccuracy from consideration. But which units shall we use?\n",
"\n",
"Obviously, we want to only have to evaluate our motion equations at integral points in time. This suggests the use of the \"cycle\" as our time unit. This leaves the distance unit to be determined.\n",
"\n",
"At first blush, it seems to make sense to use the motor step size as a distance unit. However, that is only really convenient for measuring *output*; most of the time the control loop will manage fractional steps. So, we need to find something different. Looked at from this angle, we look again at our motion equation:\n",
"\n",
"$$P = \\iiint j\\,dt = \\frac{1}{6}jt^3 + \\frac{1}{2}a_0t^2 + v_0t + p_0$$\n",
"$$a = jt + a_0$$\n",
"$$v = \\frac{1}{2}jt^2 + a_0 t+ v_0$$\n",
"\n",
"A small bit of thought is sufficient to show that, assuming that $t$ is always integral, $a$ is always an integral multiple of $j$; $v$ is an integral multiple of $\\frac{j}{2}$, and $p$ is always an integral multiple of $\\frac{j}{6}$\n",
"\n",
"Thus, our fundamental distance unit is that such that $j_{max} = 6$"
]
},
{
"cell_type": "markdown",
"id": "8cca2b8a-42a6-41c7-b0c6-eed4fd33342b",
"metadata": {},
"source": [
"## Polynomial evaluation\n",
"\n",
"As our position equation is a polynomial, we can evaluate it quickly at successive positions using the method of finite differences.\n",
"A semi-naiive evaluation of the formula would take 6 multiplications and 4 additions, whereas FD results in only the four additions.\n",
"\n",
"The differences can be calculated as follows:"
]
},
{
"cell_type": "code",
"execution_count": 21,
"id": "f8c41a01-fe9c-495c-8644-600ce88ee2e8",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left\\{ FD_{0} : \\frac{a_{0}}{2} + \\frac{j}{6} + v_{0}, \\ FD_{1} : a_{0} + j, \\ FD_{2} : j\\right\\}$"
],
"text/plain": [
"⎧ a₀ j ⎫\n",
"⎨FD₀: ── + ─ + v₀, FD₁: a₀ + j, FD₂: j⎬\n",
"⎩ 2 6 ⎭"
]
},
"execution_count": 21,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"eq1 = seg_d()['dp']\n",
"def fd(eqn, var=t):\n",
" return (eqn.subs(var, var+1) - eqn).simplify()\n",
" \n",
"fd1 = fd(eq1)\n",
"fd2 = fd(fd1)\n",
"fd3 = fd(fd2)\n",
"\n",
"{sympy.Symbol(f\"FD_{i}\"): e.subs(t,0).simplify() for i, e in enumerate([fd1, fd2, fd3])}"
]
},
{
"cell_type": "markdown",
"id": "d8b4e4c3-f76f-4de7-a570-c28a462f0283",
"metadata": {},
"source": [
"## Resulting inaccuracy\n",
"\n",
"Obviously, the step size and optimal segment times calculated above will not be integral. This means that some fudging is necessary to move the correct amount, and some more fudging might be necessary to actually reach the maximum velocity. As a first approximation of this fudging, it is simple to calculate how much "
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "dbacdbe8-1ced-4bf5-872e-e68a1b9ff7db",
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.9.10"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

22
docs/shell.nix Normal file
View File

@@ -0,0 +1,22 @@
{ pkgs ? import <nixpkgs> {} }:
let
pythonPackages = pkgs.python3.withPackages (p: with p; [
ipython
jupyterlab
scipy
sympy
bokeh
bkcharts
]);
in
pkgs.mkShell {
buildInputs = [
pythonPackages
# keep this line if you use bash
pkgs.bashInteractive
];
}