diff --git a/docs/motion-control.ipynb b/docs/motion-control.ipynb
index 802c642..7b2f5e7 100644
--- a/docs/motion-control.ipynb
+++ b/docs/motion-control.ipynb
@@ -649,7 +649,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.9.10"
+   "version": "3.9.11"
   }
  },
  "nbformat": 4,
diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock
index 0e22394..bf85884 100644
--- a/firmware/Cargo.lock
+++ b/firmware/Cargo.lock
@@ -2,6 +2,229 @@
 # It is not intended for manual editing.
 version = 3
 
+[[package]]
+name = "ansi_term"
+version = "0.12.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d52a9bb7ec0cf484c551830a7ce27bd20d67eac647e1befb56b0be4ee39a55d2"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "atty"
+version = "0.2.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8"
+dependencies = [
+ "hermit-abi",
+ "libc",
+ "winapi",
+]
+
+[[package]]
+name = "bitflags"
+version = "1.3.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
+
+[[package]]
+name = "clap"
+version = "2.34.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0610544180c38b88101fecf2dd634b174a62eef6946f84dfc6a7127512b381c"
+dependencies = [
+ "ansi_term",
+ "atty",
+ "bitflags",
+ "strsim",
+ "textwrap",
+ "unicode-width",
+ "vec_map",
+]
+
+[[package]]
+name = "heck"
+version = "0.3.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6d621efb26863f0e9924c6ac577e8275e5e6b77455db64ffa6c65c904e9e132c"
+dependencies = [
+ "unicode-segmentation",
+]
+
+[[package]]
+name = "hermit-abi"
+version = "0.1.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33"
+dependencies = [
+ "libc",
+]
+
 [[package]]
 name = "jerk_control"
 version = "0.1.0"
+
+[[package]]
+name = "lazy_static"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
+
+[[package]]
+name = "libc"
+version = "0.2.123"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cb691a747a7ab48abc15c5b42066eaafde10dc427e3b6ee2a1cf43db04c763bd"
+
+[[package]]
+name = "motion-tester"
+version = "0.1.0"
+dependencies = [
+ "jerk_control",
+ "structopt",
+]
+
+[[package]]
+name = "proc-macro-error"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
+dependencies = [
+ "proc-macro-error-attr",
+ "proc-macro2",
+ "quote",
+ "syn",
+ "version_check",
+]
+
+[[package]]
+name = "proc-macro-error-attr"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "version_check",
+]
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.37"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ec757218438d5fda206afc041538b2f6d889286160d649a86a24d37e1235afd1"
+dependencies = [
+ "unicode-xid",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a1feb54ed693b93a84e14094943b84b7c4eae204c512b7ccb95ab0c66d278ad1"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "strsim"
+version = "0.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ea5119cdb4c55b55d432abb513a0429384878c15dde60cc77b1c99de1a95a6a"
+
+[[package]]
+name = "structopt"
+version = "0.3.26"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0c6b5c64445ba8094a6ab0c3cd2ad323e07171012d9c98b0b15651daf1787a10"
+dependencies = [
+ "clap",
+ "lazy_static",
+ "structopt-derive",
+]
+
+[[package]]
+name = "structopt-derive"
+version = "0.4.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dcb5ae327f9cc13b68763b5749770cb9e048a99bd9dfdfa58d0cf05d5f64afe0"
+dependencies = [
+ "heck",
+ "proc-macro-error",
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "syn"
+version = "1.0.91"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b683b2b825c8eef438b77c36a06dc262294da3d5a5813fac20da149241dcd44d"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-xid",
+]
+
+[[package]]
+name = "textwrap"
+version = "0.11.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d326610f408c7a4eb6f51c37c330e496b08506c9457c9d34287ecc38809fb060"
+dependencies = [
+ "unicode-width",
+]
+
+[[package]]
+name = "unicode-segmentation"
+version = "1.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7e8820f5d777f6224dc4be3632222971ac30164d4a258d595640799554ebfd99"
+
+[[package]]
+name = "unicode-width"
+version = "0.1.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3ed742d4ea2bd1176e236172c8429aaf54486e7ac098db29ffe6529e0ce50973"
+
+[[package]]
+name = "unicode-xid"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ccb82d61f80a663efe1f787a51b16b5a51e3314d6ac365b08639f52387b33f3"
+
+[[package]]
+name = "vec_map"
+version = "0.8.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191"
+
+[[package]]
+name = "version_check"
+version = "0.9.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f"
+
+[[package]]
+name = "winapi"
+version = "0.3.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419"
+dependencies = [
+ "winapi-i686-pc-windows-gnu",
+ "winapi-x86_64-pc-windows-gnu",
+]
+
+[[package]]
+name = "winapi-i686-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6"
+
+[[package]]
+name = "winapi-x86_64-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml
index a9e0ceb..07f73d0 100644
--- a/firmware/Cargo.toml
+++ b/firmware/Cargo.toml
@@ -1,2 +1,2 @@
 [workspace]
-members = [ "jerk_control" ]
+members = [ "jerk_control", "motion-tester" ]
diff --git a/firmware/jerk_control/src/executor.rs b/firmware/jerk_control/src/executor.rs
new file mode 100644
index 0000000..5c485d4
--- /dev/null
+++ b/firmware/jerk_control/src/executor.rs
@@ -0,0 +1,66 @@
+use std::cmp::Ordering;
+use std::num::Wrapping;
+use crate::planner::{Profile, Segment};
+
+#[derive(Default, Debug)]
+pub struct CommandQueue {
+    cur_segment: u8,
+    cur_time: Wrapping<u32>,
+    p: Wrapping<i32>,
+    step_size: i32,
+
+    segments: [Segment; 8],
+}
+
+
+impl CommandQueue {
+    pub fn new() -> Self {
+        CommandQueue::default()
+    }
+
+    pub fn set_step_size(&mut self, step_size: u32) {
+        self.step_size = step_size as i32;
+    }
+
+    pub fn set_profile(&mut self, profile: &Profile) {
+        self.cur_segment = 0;
+        self.segments[0..8].copy_from_slice(profile.segments.as_slice());
+        self.cur_time = Wrapping(0);
+    }
+
+    pub fn running(&self) -> bool {
+        self.segments[self.cur_segment as usize].delta.iter().any(|d| d.0 != 0)
+    }
+
+    // Returns true if step signal should be high
+    pub fn step(&mut self) -> bool {
+        let next_seg_idx = (self.cur_segment + 1) % self.segments.len() as u8;
+        let next_seg_start = self.segments[next_seg_idx as usize].start_time;
+
+        let cur_segment = &mut self.segments[self.cur_segment as usize];
+
+        self.p.0 = (self.p + cur_segment.delta[0]).0.rem_euclid(self.step_size);
+
+        cur_segment.delta[0] += cur_segment.delta[1];
+        cur_segment.delta[1] += cur_segment.delta[2];
+
+        // Advance the segment if necessary.
+        self.cur_time += Wrapping(1);
+        if self.cur_time.0 >= next_seg_start && next_seg_start != 0 {
+            self.cur_segment = next_seg_idx;
+            cur_segment.start_time = 0;
+        }
+
+
+        // return state of step line
+        return self.p.0 < self.step_size / 2;
+    }
+
+    pub fn direction(& self) -> bool {
+        self.segments[self.cur_segment as usize].delta[0].0 >= 0
+    }
+
+    pub fn time(&self) -> u32 {
+        self.cur_time.0
+    }
+}
\ No newline at end of file
diff --git a/firmware/jerk_control/src/lib.rs b/firmware/jerk_control/src/lib.rs
new file mode 100644
index 0000000..52987a0
--- /dev/null
+++ b/firmware/jerk_control/src/lib.rs
@@ -0,0 +1,2 @@
+pub mod planner;
+pub mod executor;
\ No newline at end of file
diff --git a/firmware/jerk_control/src/main.rs b/firmware/jerk_control/src/main.rs
deleted file mode 100644
index ff0daec..0000000
--- a/firmware/jerk_control/src/main.rs
+++ /dev/null
@@ -1,21 +0,0 @@
-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);
-
-}
diff --git a/firmware/jerk_control/src/motion/planner.rs b/firmware/jerk_control/src/planner.rs
similarity index 86%
rename from firmware/jerk_control/src/motion/planner.rs
rename to firmware/jerk_control/src/planner.rs
index 482f570..0071c21 100644
--- a/firmware/jerk_control/src/motion/planner.rs
+++ b/firmware/jerk_control/src/planner.rs
@@ -35,14 +35,14 @@ pub struct Planner {
 
 #[derive(Copy, Clone, Debug)]
 pub struct Profile {
-    segments: [Segment; 7]
+    pub segments: [Segment; 8]
 }
 
 #[derive(Copy, Clone, Debug, Default)]
 pub struct Segment {
     // Used by executor
-    pub delta: [i32; 3],
-    start_time: u32, // 0 to disable; set after completion.
+    pub delta: [Wrapping<i32>; 3],
+    pub start_time: u32, // 0 to disable; set after completion.
     // used by planner
     v0: i32,
     a0: i32,
@@ -52,7 +52,7 @@ pub struct Segment {
 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 j = self.delta[2].0; // 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
@@ -87,15 +87,11 @@ 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) + (j / 6) + self.v0),
-                self.a0 + j,
-                j,
+                Wrapping((self.a0 / 2) + (j / 6) + self.v0),
+                Wrapping(self.a0 + j),
+                Wrapping(j),
             ],
             start_time: self.time,
             v0: self.v0,
@@ -104,7 +100,10 @@ impl State {
         }
     }
 
-    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Segment {
+    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Option<Segment> {
+        if length == 0 {
+            return None;
+        }
         let segment = self.segment_for(j);
         let t = length as i32;
         let t2 = t * length as i32;
@@ -119,7 +118,7 @@ impl State {
 
         self.time += length as u32;
 
-        segment
+        Some(segment)
     }
 }
 
@@ -173,7 +172,7 @@ impl Planner {
 
         // 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;
+        self.ta_max = (self.v_max / self.a_max).checked_sub(self.tj_max).unwrap_or(0);
 
         // Compute regime change points
         let a_max_2 = self.a_max as u64 * self.a_max as u64;
@@ -213,8 +212,9 @@ impl Planner {
         let tv = (dx - ramp_dx) * 2 / s4v as u64;
         let tv = ((tv + 1) / 2) as u32;
 
+        eprintln!("ta={ta}, tj={tj}, tv={tv}");
         let j_real = dir * 6;
-        let mut segments = [Segment::default(); 7];
+        let mut segments = [Segment::default(); 8];
         let seg_params = [
             (j_real, tj),
             (0, ta),
@@ -224,9 +224,19 @@ impl Planner {
             (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);
-        }
+
+        let step_size = self.step_size;
+
+
+        let mut imax = 0;
+        let seg_count =
+            seg_params.iter()
+                .copied()
+                .filter_map(|(j,t)| cstate.produce_segment(j, t,  step_size))
+                .enumerate()
+                .map(|(i, segment)| segments[i] = segment)
+                .count();
+        segments[seg_count].start_time = tj * 4 + ta * 2 + tv;
         Profile {
             segments,
         }