Started to move to submodule-based setup, to test motion control outside of firmware

firmware/jerk_control/Cargo.toml

@@ -0,0 +1,8 @@
+[package]
+name = "jerk_control"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]

firmware/jerk_control/src/main.rs

@@ -0,0 +1,21 @@
+pub mod motion {
+    pub mod planner;
+}
+
+use motion::planner::{Planner, Config};
+use crate::motion::planner::State;
+
+fn main() {
+    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/jerk_control/src/motion/planner.rs

@@ -0,0 +1,237 @@
+//! 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 {
+    /// 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)]
+pub struct Planner {
+    tick_frequency: u32,
+    // all measurements are in jd/tick^n, where jd is the distance that makes j_max=6 and n is
+    // 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: u64,
+    xmax_ca: u64,
+
+    tj_max: u32,
+    ta_max: u32,
+}
+
+#[derive(Copy, Clone, Debug)]
+pub struct Profile {
+    segments: [Segment; 7]
+}
+
+#[derive(Copy, Clone, Debug, Default)]
+pub struct Segment {
+    // Used by executor
+    pub delta: [i32; 3],
+    start_time: u32, // 0 to disable; set after completion.
+    // used by planner
+    v0: i32,
+    a0: i32,
+    p0: u32, // 0 if the step hasn't been started.
+}
+
+impl Segment {
+    // 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)
+    }
+}
+
+#[derive(Copy, Clone, Debug, Default)]
+pub struct State {
+    time: u32,
+    p0: u32,
+    v0: i32,
+    a0: i32,
+}
+
+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) + (j / 6) + self.v0),
+                self.a0 + j,
+                j,
+            ],
+            start_time: self.time,
+            v0: self.v0,
+            a0: self.a0,
+            p0: self.p0,
+        }
+    }
+
+    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Segment {
+        let segment = self.segment_for(j);
+        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;
+
+        self.time += length as u32;
+
+        segment
+    }
+}
+
+impl Planner {
+    // Note that this uses the `recconfigure` method to apply the configuration, and thus will
+    // clamp v_max such that
+    pub fn new(tick_frequency: u32, config: Config) -> Option<Self> {
+        let mut ret = Self {
+            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) {
+            Some(ret)
+        } else {
+            None
+        }
+    }
+
+    /// Reconfigure the planner to use the given speeds. Note that this should only be run while
+    /// stopped.
+    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, (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 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;
+    }
+
+    // Note that dx is in internal units
+    pub fn plan_profile(&self, dx: i64, state: State) -> Profile {
+        let mut cstate = state;
+
+        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,
+        }
+    }
+
+    pub fn step_size(&self) -> u32 { self.step_size }
+
+}