Progress towards firmware

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": []
   }

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" }

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<u32>,
+        pub profile: planner::Profile,
+    }
+    /// Manages communication with the execution interrupt
+    struct StepperComm {
+        pub position_steps: Wrapping<u32>,
+        pub position_substep: Wrapping<u32>,
+        pub cur_time: Wrapping<u32>,
+        pub next_profile: Option<PlannedProfile>,
+        pub active: bool,
+    }
+
+    pub struct StepperProcess<Pin> {
+        pub executor: executor::CommandQueue,
+        pub pin: Pin,
+    }
+
     #[shared]
     struct Shared {
 
@@ -70,7 +90,9 @@ mod app {
     }
 
     #[local]
-    struct Local {}
+    struct Local {
+        executor1: StepperProcess<rp_pico::hal::gpio::Pin<hal::gpio::pin::bank0::>>,
+    }
 
     #[init(local = [usb_bus: Option<usb_device::bus::UsbBusAllocator<hal::usb::UsbBus>> = 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],
     )]

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
     }
-}
+}

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 }
 
-}
+}