Got the firmware to compile and produce something vaguely sensible.

firmware/.envrc

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

firmware/docs/implementation-notes.adoc

@@ -1,6 +1,6 @@
 Motion control in an interrupt.
 
-= Parts
+== Parts
 This consists of two parts, the planner and the executor.
 
 The planner receives target positions. Each time it receives a target
@@ -13,7 +13,7 @@ the step lines of the MCU.
 
 These two processes communicate by means of a command queue.
 
-== Executor
+=== Executor
 1. Update a cycle counter
 2. Evaluates the next output of the position polynomial (3 adds)
 3. determine whether to toggle a stepper, and do so.
@@ -21,7 +21,7 @@ These two processes communicate by means of a command queue.
 
 
 
-== Command queue
+=== Command queue
 
 The command queue takes the form of a ring buffer, with each item
 containing a motion segment.  The ring buffer must be large enough to
@@ -40,9 +40,9 @@ A motion profile segment consists of the following values:
 The following invariants hold for the command queue:
 
 
-== Planner
+=== Planner
 
-=== Aborting
+==== Aborting
 
 In case of an abort, the fastest stop profile will consist of at most
 3 segments: const -jerk to max -a, const -a to to lead-out, const +j

firmware/shell.nix

@@ -0,0 +1,10 @@
+{ pkgs ? import <nixpkgs> {} }:
+
+pkgs.mkShell {
+  buildInputs = [
+    pkgs.stdenv
+
+    # keep this line if you use bash
+    pkgs.bashInteractive
+  ];
+}

firmware/src/main.rs

@@ -2,6 +2,20 @@ pub mod motion {
     pub mod planner;
 }
 
+use motion::planner::{Planner, Config};
+use crate::motion::planner::State;
+
 fn main() {
-    println!("Hello, world!");
+    let planner_config = Config {
+        j_max: 231.,
+        v_max: 0.0,
+        a_max: 100. , // WAG
+        step_size: 0.2
+    };
+    let planner = Planner::new(5_000, planner_config)
+        .expect("Planner config should succeed");
+
+    let profile = planner.plan_profile(planner.step_size() as i64 * 2500, State::default());
+    println!("Profile: {:#?}", profile);
+
 }

firmware/src/motion/planner.rs

@@ -1,9 +1,18 @@
+//! The motion planner.
+//!
+//! 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;
+
 pub struct Config {
-    j_max: f32, // in mm/s^3
-    v_max: f32, // mm/s^2
-    // mm/s
-    a_max: f32,
-    step_size: f32, // um !
+    /// Max jerk, in mm/s^3
+    pub j_max: f32,
+    /// Max velocity, in mm/s. If 0, 1 step every other tick.
+    pub v_max: f32, // mm/s
+    // Max acceleration, in mm/s^2.
+    pub a_max: f32,
+    pub step_size: f32,
 }
 
 #[derive(Debug, Clone)]
@@ -13,11 +22,15 @@ pub struct Planner {
     // the appropriate power of time for the unit.
     v_max: u32,
     a_max: u32,
+    rt2_a_max: u32, // square root of a_max
     step_size: u32,
 
     // nsteps for each regime
-    xmax_cj: u32,
-    xmax_ca: u32,
+    xmax_cj: u64,
+    xmax_ca: u64,
+
+    tj_max: u32,
+    ta_max: u32,
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -25,10 +38,10 @@ pub struct Profile {
     segments: [Segment; 7]
 }
 
-#[derive(Copy, Clone, Debug)]
+#[derive(Copy, Clone, Debug, Default)]
 pub struct Segment {
     // Used by executor
-    pub delta: [u32; 3],
+    pub delta: [i32; 3],
     start_time: u32, // 0 to disable; set after completion.
     // used by planner
     v0: i32,
@@ -37,9 +50,30 @@ pub struct Segment {
 }
 
 impl Segment {
-    pub fn state_at_time(&self, time: u32) -> State {
-        let j = self.delta[2] as i32;
-        let
+    // This will work for |t|<=65535
+    pub fn state_at_time_u64(&self, time: u32, step_size: u32) -> (i32, State) {
+        let j = self.delta[2] as i32; // 6, 0, or -6
+        let t = time as i64;
+        let t2 = (time * time) as i64;
+        // TODO: figure out what can be handled as u32, as 64-bit arithmentic is significantly slower
+        let dp = (j / 6) as i64 * t * t2 + (self.a0 / 2) as i64 * t2 + self.v0 as i64 * t;
+        let dv = (j / 2) * (t2 as i32) + self.a0 * time as i32;
+        let da = j * time as i32;
+        let time = self.start_time + time;
+        let pe = self.p0 as i64 + dp;
+
+        let p0 = pe.rem_euclid(step_size as i64) as u32;
+        let nstep = pe.div_euclid(step_size as i64) as i32;
+
+
+        let new_state = State {
+            time: self.start_time + time,
+            p0,
+            a0: self.a0 + da,
+            v0: self.v0 + dv,
+        };
+
+        (nstep, new_state)
     }
 }
 
@@ -53,11 +87,15 @@ pub struct State {
 
 impl State {
     fn segment_for(&self, j: i32) -> Segment {
+        let a0_2 = Wrapping((self.a0 / 2) as u32);
+        let a0 = Wrapping((self.a0) as u32);
+        let j_6 = Wrapping((j / 6) as u32);
+        let j_2 = Wrapping((j / 2) as u32);
         Segment {
             delta: [
-                (self.a0 / 2) as u32 + (j / 6) as u32 + self.v0 as u32,
-                self.a0 as u32 + j as u32,
-                j as u32,
+                ((self.a0 / 2) + (j / 6) + self.v0),
+                self.a0 + j,
+                j,
             ],
             start_time: self.time,
             v0: self.v0,
@@ -66,12 +104,16 @@ impl State {
         }
     }
 
-    fn produce_segment(&mut self, j: i32, length: u32) -> Segment {
+    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Segment {
         let segment = self.segment_for(j);
-        let t = length;
-        let t2 = t * length;
-        let t3 = t2 * length;
-        self.p0 += (j / 6) as u32 * t3 + (self.a0 / 2) as u32 * t2 + self.v0 as u32 * t;
+        let t = length as i32;
+        let t2 = t * length as i32;
+        let t3 = t2 as i64 * length as i64;
+        let dp = (j / 6) as i64 * t3
+            + (self.a0 / 2) as i64 * t2 as i64
+            + self.v0 as i64 * t as i64;
+
+        self.p0 = (self.p0 as i64 + dp).rem_euclid(step_size as i64) as u32;
         self.v0 += j / 2 * t2 as i32 + self.a0 * t as i32;
         self.a0 += j * t as i32;
 
@@ -89,7 +131,12 @@ impl Planner {
             tick_frequency,
             v_max: 0,
             a_max: 0,
+            rt2_a_max: 0,
             step_size: 0,
+            xmax_cj: 0,
+            xmax_ca: 0,
+            tj_max: 0,
+            ta_max: 0
         };
 
         if ret.reconfigure(config) {
@@ -104,36 +151,87 @@ impl Planner {
     pub fn reconfigure(&mut self, config: Config) -> bool {
         let tick_rate = self.tick_frequency as f32;
         let a_max = (6. * config.a_max * tick_rate / config.j_max)
-            .clamp(0.0, (1 << 32) as f32);
-        let v_max = (6. * config.v_max * tick_rate * tick_rate / config.j_max)
-            .clamp(0.0, (1 << 31) as f32);
-        let step_size = config.step_size * 6. * tick_rate * tick_rate * tick_rate / config.j_max / 1000.;
+            .clamp(0.0, (1u32 << 31) as f32);
+
+        let v_max = if config.v_max == 0. {
+            config.step_size * tick_rate / 2.
+        } else {
+            config.v_max
+        };
+
+        let v_max = (6. * v_max * tick_rate * tick_rate / config.j_max)
+            .clamp(0.0, (1u32 << 31) as f32);
+        let step_size = config.step_size * 6. * tick_rate * tick_rate * tick_rate / config.j_max;
         if step_size > (u32::MAX / 2) as f32 {
+            eprintln!("Failed to configure planner: stepsize = {}", step_size);
             return false;
         }
         self.a_max = a_max as u32;
+        self.rt2_a_max = a_max.sqrt() as u32;
         self.v_max = v_max as u32;
         self.step_size = step_size as u32;
 
+        // Compute ta_max and tj_max
+        self.tj_max = self.a_max / 6;
+        self.ta_max = self.v_max / self.a_max - self.tj_max;
+
         // Compute regime change points
-        let amax2 = self.a_max * self.a_max;
-        self.xmax_cj = self.a_max * amax2 / 18; // jmax = 6, xmax_cj = 2*amax^3/jmax^2
-        self.xmax_ca = self.a_max * self.v_max / 6 + self.v_max * self.v_max / self.a_max;
+        let a_max_2 = self.a_max as u64 * self.a_max as u64;
+        self.xmax_cj = self.a_max as u64 / 18 * a_max_2; // jmax = 6, xmax_cj = 2*amax^3/jmax^2
+        self.xmax_ca = self.a_max as u64 * self.v_max as u64 / 6 + self.v_max as u64 * self.v_max as u64 / self.a_max as u64;
+
         return true;
     }
 
-    pub fn plan_profile(&self, dx: i32, state: State) -> Profile {
+    // Note that dx is in internal units
+    pub fn plan_profile(&self, dx: i64, state: State) -> Profile {
         let mut cstate = state;
 
-        let (tj, ta, tv) =
-        if dx.abs() as u32 <= self.xmax_cj {
-            
-        };
+        let j = 6;
+        let dir = dx.signum() as i32;
+        let dx = dx.abs() as u64;
+
+
+        let (tj, ta) =
+            if dx <= self.xmax_cj as u64 {
+                let tj = f32::cbrt(dx as f32 / 2. / j as f32) as u32;
+                (tj, 0)
+            } else if dx <= self.xmax_ca as u64 {
+                let ta_quadrat =
+                    self.a_max * self.a_max / (4 * 36) // amax^3/(4*amax*jmax^2)
+                        + (dx / self.a_max as u64) as u32;
+                let ta = f32::sqrt(ta_quadrat as f32) as u32
+                    - self.a_max / 4;
+                (self.tj_max, ta)
+            } else {
+                (self.tj_max, self.ta_max)
+            };
+
+        // Now we have a value for t_j and t_a. Compute the velocity and necessary Δx during t_v
+        let s4v = j * tj * (ta + tj);
+        let ramp_dx = (j * tj) as u64 * (ta * ta + 3 * ta * tj + 2 * tj * tj) as u64;
+        let tv = (dx - ramp_dx) * 2 / s4v as u64;
+        let tv = ((tv + 1) / 2) as u32;
+
+        let j_real = dir * 6;
+        let mut segments = [Segment::default(); 7];
+        let seg_params = [
+            (j_real, tj),
+            (0, ta),
+            (-j_real, tj),
+            (0, tv),
+            (-j_real, tj),
+            (0, ta),
+            (j_real, tj),
+        ];
+        for (i, (j,t)) in seg_params.iter().copied().enumerate() {
+            segments[i] = cstate.produce_segment(j, t, self.step_size);
+        }
         Profile {
-            segments: [
-                Planner::segmentFor()
-            ]
+            segments,
         }
     }
 
+    pub fn step_size(&self) -> u32 { self.step_size }
+
 }