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

More work on the film scanner

Browse Source
This commit is contained in:
TQ Hirsch
2022-04-04 12:20:58 +02:00
parent cb97593887
commit 738473f644
1 changed files with 331 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
motion-control.ipynb
View File

@@ -7,7 +7,8 @@
"metadata": {},
"outputs": [],
"source": [
"import sympy"
"import sympy\n",
"sympy.init_printing()"
]
},
{
@@ -22,25 +23,38 @@
},
{
"cell_type": "code",
"execution_count": 66,
"execution_count": 2,
"id": "76a230d3-e50b-450e-bd40-47d7a9632043",
"metadata": {},
"outputs": [],
"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\", real=True)\n",
"g_a0 = a0\n",
"g_v0 = v0\n",
"g_t = t"
"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": 67,
"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):\n",
"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",
@@ -56,7 +70,7 @@
},
{
"cell_type": "code",
"execution_count": 68,
"execution_count": 4,
"id": "378b1d65-afdb-496a-8217-f17ce9e92783",
"metadata": {},
"outputs": [
@@ -66,10 +80,13 @@
"$\\displaystyle p = \\frac{t \\left(3 a_{0} t + j t^{2} + 6 v_{0}\\right)}{6}$"
],
"text/plain": [
"Eq(p, t*(3*a0*t + j*t**2 + 6*v0)/6)"
" ⎛ 2 ⎞\n",
" t⋅⎝3⋅a₀⋅t + j⋅t + 6⋅v₀⎠\n",
"p = ────────────────────────\n",
" 6 "
]
},
"execution_count": 68,
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
@@ -115,134 +132,68 @@
},
{
"cell_type": "code",
"execution_count": 96,
"execution_count": 5,
"id": "a1722a31-0b31-4cfd-8179-4222fdb2fb56",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'dt': a_max/j_max,\n",
" 'da': a_max,\n",
" 'dv': a_max**2/(2*j_max),\n",
" 'dp': a_max**3/(6*j_max**2),\n",
" 'ae': a_max,\n",
" 've': a_max**2/(2*j_max),\n",
" 'pe': a_max**3/(6*j_max**2)}"
]
},
"execution_count": 96,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"# Time and position calculations for general form\n",
"t_a = a_max / j_max\n",
"p1s1 = seg_d(j=j_max, a0=0, v0=0, t=t_a)\n",
"p1s1"
"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": 97,
"id": "56e480b7-0f00-4791-a443-b4891bb522b1",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'dt': a_max/j_max,\n",
" 'da': -a_max,\n",
" 'dv': a_max**2/(2*j_max),\n",
" 'dp': -a_max**3/(6*j_max**2) + a_max*v_max/j_max,\n",
" 'ae': 0,\n",
" 've': v_max,\n",
" 'pe': -a_max**3/(6*j_max**2) + a_max*v_max/j_max}"
]
},
"execution_count": 97,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s3 = seg_d(j=-j_max, a0=p1s1[\"ae\"], t=t_a)\n",
"p1s3_v0 = sympy.solve(p1s3[\"ve\"] - v_max, v0)[0]\n",
"p1s3 = seg_d(j=-j_max, a0=p1s1[\"ae\"], t=t_a, v0 = p1s3_v0)\n",
"p1s3"
]
},
{
"cell_type": "code",
"execution_count": 98,
"execution_count": 6,
"id": "9554e8a3-f968-4ae2-b0b0-52e0e6489631",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'dt': -a_max/j_max + v_max/a_max,\n",
" 'da': 0,\n",
" 'dv': -a_max**2/j_max + v_max,\n",
" 'dp': v_max*(-a_max**2 + j_max*v_max)/(2*a_max*j_max),\n",
" 'ae': a_max,\n",
" 've': -a_max**2/(2*j_max) + v_max,\n",
" 'pe': a_max**3/(6*j_max**2) - a_max*v_max/(2*j_max) + v_max**2/(2*a_max)}"
]
},
"execution_count": 98,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"t_v = (p1s3_v0 - p1s1[\"ve\"]) / p1s1[\"ae\"]\n",
"p1s2 = seg_d(j = 0, a0=p1s1[\"ae\"], v0 = p1s1[\"ve\"], t = t_v, p0=p1s1[\"pe\"])\n",
"p1s3[\"pe\"] = (p1s3[\"dp\"] + p1s2[\"pe\"]).simplify()\n",
"p1s2"
"p1s2 = seg_d(j = 0, t = t_a, following=p1s1)\n",
"# p1s2"
]
},
{
"cell_type": "code",
"execution_count": 99,
"id": "61ae6edb-d153-4f71-aa0f-fe1b6b882e2b",
"execution_count": 7,
"id": "56e480b7-0f00-4791-a443-b4891bb522b1",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\frac{a_{max}^{3}}{6 j_{max}^{2}}$"
],
"text/plain": [
"a_max**3/(6*j_max**2)"
]
},
"execution_count": 99,
"metadata": {},
"output_type": "execute_result"
}
],
"outputs": [],
"source": [
"p1s1_dp.subs(a_max, None)"
"p1s3 = seg_d(j=-j_max, t=t_j, following=p1s2)\n",
"# p1s3"
]
},
{
"cell_type": "code",
"execution_count": 100,
"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{v_{max} \\left(a_{max}^{2} + j_{max} v_{max}\\right)}{2 a_{max} j_{max}}$"
"$\\displaystyle \\frac{j_{max} t_{j} \\left(t_{a}^{2} + 3 t_{a} t_{j} + 2 t_{j}^{2}\\right)}{2}$"
],
"text/plain": [
"v_max*(a_max**2 + j_max*v_max)/(2*a_max*j_max)"
" ⎛ 2 2⎞\n",
"jₘₐₓ⋅t_j⋅⎝tₐ + 3⋅tₐ⋅t_j + 2⋅t_j ⎠\n",
"──────────────────────────────────\n",
" 2 "
]
},
"execution_count": 100,
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
@@ -253,9 +204,62 @@
},
{
"cell_type": "code",
"execution_count": 108,
"id": "f2ae9840-53ff-4295-8555-fc5ae26affcf",
"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": {
@@ -266,85 +270,265 @@
"0"
]
},
"execution_count": 108,
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"(p1s3[\"pe\"] - sum(x[\"dp\"] for x in [p1s5, p1s6, p1s7])).simplify()"
"(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": 106,
"id": "b81e62d7-62ec-4d11-8ba4-c9c8d460ebfb",
"metadata": {},
"outputs": [],
"source": [
"p1s5 = seg_d(j=-j_max, a0=0, v0=v_max, t=t_a, p0=p1s3[\"pe\"])\n",
"p1s6 = seg_d(j=0, a0=p1s5[\"ae\"], v0=p1s5[\"ve\"], t=t_v, p0=p1s5[\"pe\"])\n",
"p1s7 = seg_d(j=j_max, a0=p1s6[\"ae\"], v0=p1s6[\"ve\"], t=t_a, p0=p1s6[\"pe\"])"
]
},
{
"cell_type": "code",
"execution_count": 107,
"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": [
"{'dt': a_max/j_max,\n",
" 'da': a_max,\n",
" 'dv': -a_max**2/(2*j_max),\n",
" 'dp': a_max**3/(6*j_max**2),\n",
" 'ae': 0,\n",
" 've': 0,\n",
" 'pe': a_max*v_max/j_max + v_max**2/a_max}"
" 2\n",
"aₘₐₓ⋅vₘₐₓ vₘₐₓ \n",
"───────── + tᵥ⋅vₘₐₓ + ─────\n",
" jₘₐₓ aₘₐₓ"
]
},
"execution_count": 107,
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s7"
"p1s7[\"pe\"].subs({t_j: t_j_max, t_a: t_a_max}).simplify()"
]
},
{
"cell_type": "code",
"execution_count": 109,
"id": "3b0efbae-6e97-406c-849f-e627fe388335",
"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": [
"{'dt': a_max/j_max,\n",
" 'da': -a_max,\n",
" 'dv': a_max**2/(2*j_max),\n",
" 'dp': -a_max**3/(6*j_max**2) + a_max*v_max/j_max,\n",
" 'ae': 0,\n",
" 've': v_max,\n",
" 'pe': v_max*(a_max**2 + j_max*v_max)/(2*a_max*j_max)}"
" aₘₐₓ p vₘₐₓ\n",
"tᵥ = - ──── + ──── - ────\n",
" jₘₐₓ vₘₐₓ aₘₐₓ"
]
},
"execution_count": 109,
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"p1s3"
"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": null,
"id": "739d944e-19ae-4b8c-866b-f430c11b1576",
"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 = int(\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": "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": []