diff --git a/docs/motion-control.ipynb b/docs/motion-control.ipynb index 7b2f5e7..6f69fb8 100644 --- a/docs/motion-control.ipynb +++ b/docs/motion-control.ipynb @@ -318,6 +318,32 @@ "p3 = p1s7[\"pe\"].subs({t_a: 0, t_v: 0}).simplify()" ] }, + { + "cell_type": "code", + "execution_count": 36, + "id": "06638377-bd46-4b62-84d5-05271c6ef7ab", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle - \\frac{a_{max}}{j_{max}} + \\frac{v_{max}}{a_{max}}$" + ], + "text/plain": [ + " aₘₐₓ vₘₐₓ\n", + "- ──── + ────\n", + " jₘₐₓ aₘₐₓ" + ] + }, + "execution_count": 36, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "t_a_max" + ] + }, { "cell_type": "code", "execution_count": 16, @@ -347,7 +373,7 @@ }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 38, "id": "5f779934-7f89-4ef4-9f89-d268a9742059", "metadata": {}, "outputs": [ @@ -365,43 +391,69 @@ " 2⋅╲╱ aₘₐₓ ⋅jₘₐₓ " ] }, - "execution_count": 17, + "execution_count": 38, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "p2ta = sympy.solve(p - p2, t_a, positive=True)[0]\n", + "p2ta = sympy.solve(p - p2, t_a, positive=True)[0].simplify()\n", "sympy.Eq(t_a, p2ta)" ] }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 55, + "id": "a0a4fc4f-e5a5-4c91-b8be-cc80f710438f", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{a_{max}^{3} + 4 j_{max}^{2} p}{4 a_{max} j_{max}^{2}}$" + ], + "text/plain": [ + " 3 2 \n", + "aₘₐₓ + 4⋅jₘₐₓ ⋅p\n", + "─────────────────\n", + " 2 \n", + " 4⋅aₘₐₓ⋅jₘₐₓ " + ] + }, + "execution_count": 55, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "(p2ta + 3*(a_max ** sympy.Rational(3,2)) / (2 * a_max ** sympy.Rational(1,2) * j_max)).simplify() ** 2" + ] + }, + { + "cell_type": "code", + "execution_count": 35, "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}}}$" + "$\\displaystyle t_{j} = \\frac{p}{2 j_{max}}$" ], "text/plain": [ - " 2/3 3 ___\n", - " 2 ⋅╲╱ p \n", - "t_j = ──────────\n", - " 3 ______\n", - " 2⋅╲╱ jₘₐₓ " + " p \n", + "t_j = ──────\n", + " 2⋅jₘₐₓ" ] }, - "execution_count": 18, + "execution_count": 35, "metadata": {}, "output_type": "execute_result" } ], "source": [ "p3tj = sympy.solve(p - p3, t_j)[0]\n", - "sympy.Eq(t_j, p3tj)" + "sympy.Eq(t_j, (p3tj**3))" ] }, { @@ -626,9 +678,36 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 61, "id": "dbacdbe8-1ced-4bf5-872e-e68a1b9ff7db", "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": 61, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "p1s3[\"pe\"]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a63210fc-0c10-43ad-b78e-ec3b0814b8e1", + "metadata": {}, "outputs": [], "source": [] } diff --git a/firmware/film-scanner-fw/Cargo.toml b/firmware/film-scanner-fw/Cargo.toml index 63cea4c..5ed75d2 100644 --- a/firmware/film-scanner-fw/Cargo.toml +++ b/firmware/film-scanner-fw/Cargo.toml @@ -35,3 +35,5 @@ rp2040-hal = { version="0.4.0", features=["rt"] } rp2040-boot2 = {version="0.2.0", optional = true } +# Film scanner-specific stuff +jerk_control = { path = "../jerk_control" } diff --git a/firmware/film-scanner-fw/src/main.rs b/firmware/film-scanner-fw/src/main.rs index 777ee58..2f29023 100644 --- a/firmware/film-scanner-fw/src/main.rs +++ b/firmware/film-scanner-fw/src/main.rs @@ -4,6 +4,7 @@ use defmt_rtt as _; use panic_probe as _; + mod fmt; #[rtic::app(device = rp_pico::hal::pac, peripherals = true)] @@ -11,6 +12,7 @@ mod app { use crate::fmt::Wrapper; use core::fmt::Write; + use core::num::Wrapping; use embedded_hal::digital::v2::OutputPin; use embedded_time::duration::Extensions; @@ -24,6 +26,8 @@ mod app { // USB Communications Class Device support use usbd_serial::SerialPort; + use jerk_control::{planner, executor}; + // Blinck time 5 seconds const SCAN_TIME_US: u32 = 5000000; // 200000; // 5000000; // 1000000; // 200000; @@ -49,11 +53,27 @@ mod app { // Mathias // https://github.com/mgottschlag/rp2040-usb-sound-card/blob/b8078b57361c1b08755e5ab5f9992c56457ec18b/src/main.rs#L188 // - // - // Global Static variable, has to be written inside unsafe blocks. - // A reference can be obtained with as_ref() method. + /// A profile that is to be loaded at a certain point in time + struct PlannedProfile { + pub apply_time: Wrapping, + pub profile: planner::Profile, + } + /// Manages communication with the execution interrupt + struct StepperComm { + pub position_steps: Wrapping, + pub position_substep: Wrapping, + pub cur_time: Wrapping, + pub next_profile: Option, + pub active: bool, + } + + pub struct StepperProcess { + pub executor: executor::CommandQueue, + pub pin: Pin, + } + #[shared] struct Shared { @@ -70,7 +90,9 @@ mod app { } #[local] - struct Local {} + struct Local { + executor1: StepperProcess>, + } #[init(local = [usb_bus: Option> = None])] fn init(c: init::Context) -> (Shared, Local, init::Monotonics) { @@ -164,7 +186,7 @@ mod app { /// Task that blinks the rp-pico onboard LED and that send a message "LED ON!" and "LED OFF!" do USB-Serial. #[task( binds = TIMER_IRQ_0, - priority = 1, + priority = 4, shared = [timer, alarm, serial], local = [tog: bool = true], )] diff --git a/firmware/jerk_control/src/executor.rs b/firmware/jerk_control/src/executor.rs index 1efd58c..532a68e 100644 --- a/firmware/jerk_control/src/executor.rs +++ b/firmware/jerk_control/src/executor.rs @@ -1,5 +1,6 @@ -use std::cmp::Ordering; -use std::num::Wrapping; +use core::cmp::Ordering; +use core::num::Wrapping; +use core::option::Option; use crate::planner::{Profile, Segment}; #[derive(Default, Debug)] @@ -75,4 +76,4 @@ impl CommandQueue { pub fn time(&self) -> u32 { self.cur_time.0 } -} \ No newline at end of file +} diff --git a/firmware/jerk_control/src/planner.rs b/firmware/jerk_control/src/planner.rs index 0071c21..6da8f9d 100644 --- a/firmware/jerk_control/src/planner.rs +++ b/firmware/jerk_control/src/planner.rs @@ -3,7 +3,7 @@ //! Note that the equations in this file are rather complex and non-obvious. //! All of their derivations can be found in the motion-control.ipynb file. -use std::num::Wrapping; +use core::num::Wrapping; pub struct Config { /// Max jerk, in mm/s^3 @@ -244,4 +244,4 @@ impl Planner { pub fn step_size(&self) -> u32 { self.step_size } -} \ No newline at end of file +}