Started writing firmware

firmware/.gitignore

@@ -0,0 +1,3 @@
+/target
+.idea
+*.iml

firmware/Cargo.lock

@@ -0,0 +1,7 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 3
+
+[[package]]
+name = "film-scanner-fw"
+version = "0.1.0"

firmware/Cargo.toml

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

firmware/src/main.rs

@@ -0,0 +1,7 @@
+pub mod motion {
+    pub mod planner;
+}
+
+fn main() {
+    println!("Hello, world!");
+}

firmware/src/motion/planner.rs

@@ -0,0 +1,139 @@
+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 !
+}
+
+#[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,
+    step_size: u32,
+
+    // nsteps for each regime
+    xmax_cj: u32,
+    xmax_ca: u32,
+}
+
+#[derive(Copy, Clone, Debug)]
+pub struct Profile {
+    segments: [Segment; 7]
+}
+
+#[derive(Copy, Clone, Debug)]
+pub struct Segment {
+    // Used by executor
+    pub delta: [u32; 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 {
+    pub fn state_at_time(&self, time: u32) -> State {
+        let j = self.delta[2] as i32;
+        let
+    }
+}
+
+#[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 {
+        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,
+            ],
+            start_time: self.time,
+            v0: self.v0,
+            a0: self.a0,
+            p0: self.p0,
+        }
+    }
+
+    fn produce_segment(&mut self, j: i32, length: 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;
+        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,
+            step_size: 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, (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.;
+        if step_size > (u32::MAX / 2) as f32 {
+            return false;
+        }
+        self.a_max = a_max as u32;
+        self.v_max = v_max as u32;
+        self.step_size = step_size as u32;
+
+        // 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;
+        return true;
+    }
+
+    pub fn plan_profile(&self, dx: i32, state: State) -> Profile {
+        let mut cstate = state;
+
+        let (tj, ta, tv) =
+        if dx.abs() as u32 <= self.xmax_cj {
+            
+        };
+        Profile {
+            segments: [
+                Planner::segmentFor()
+            ]
+        }
+    }
+
+}