diff --git a/firmware/motion-tester/Cargo.toml b/firmware/motion-tester/Cargo.toml
new file mode 100644
index 0000000..1e48dda
--- /dev/null
+++ b/firmware/motion-tester/Cargo.toml
@@ -0,0 +1,10 @@
+[package]
+name = "motion-tester"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+jerk_control = { path = "../jerk_control" }
+structopt = { version = "0.3.26" }
\ No newline at end of file
diff --git a/firmware/motion-tester/src/main.rs b/firmware/motion-tester/src/main.rs
new file mode 100644
index 0000000..e32944b
--- /dev/null
+++ b/firmware/motion-tester/src/main.rs
@@ -0,0 +1,58 @@
+use structopt::StructOpt;
+use jerk_control::executor::CommandQueue;
+use jerk_control::planner::{Config, Planner, State};
+
+#[derive(StructOpt)]
+struct Cli {
+    #[structopt(long = "freq", short="f", default_value = "5000")]
+    step_frequency: u32,
+    #[structopt(long, short)]
+    j_max: f32,
+    #[structopt(long, short)]
+    a_max: f32,
+    #[structopt(default_value="0", long, short)]
+    v_max: f32,
+    #[structopt(default_value = "0.2", long="step", short)]
+    step_size: f32,
+    #[structopt(long="dist", short)]
+    distance: f32,
+}
+
+fn main() {
+    let cli = <Cli as StructOpt>::from_args();
+
+    let planner_config = Config {
+        j_max: cli.j_max,
+        v_max: cli.v_max,
+        a_max: cli.a_max, // WAG
+        step_size: cli.step_size,
+    };
+    let planner = Planner::new(cli.step_frequency, planner_config)
+        .expect("Planner config should succeed");
+
+    // eprintln!("Planner: {:#?}", &planner);
+
+    let mm_per_iunit = cli.step_size / planner.step_size() as f32;
+    // eprintln!("mm/iu = {}", mm_per_iunit);
+
+    let profile = planner.plan_profile( (cli.distance / mm_per_iunit) as i64, State::default());
+
+    // Run simulation
+    let mut executor = CommandQueue::new();
+    executor.set_step_size(planner.step_size());
+    executor.set_profile(&profile);
+
+    let mut pos = 0;
+    let mut last_step = false;
+
+    while executor.running() {
+        let dir = executor.direction();
+        let next_step = executor.step();
+        if next_step & !last_step {
+            pos += if dir { 1 } else { -1 };
+            // print a log record.
+            println!("{},{}", executor.time() as f32 / cli.step_frequency as f32, pos as f32 * cli.step_size);
+        }
+        last_step = next_step;
+    }
+}